From 85ca81d12abedacb5e0f9f0770d2514b6f987e79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Tue, 5 Mar 2019 18:12:38 +0100 Subject: [PATCH] robot_state, fix logging Adapt code to new logging system as discussed at https://github.com/ros-planning/moveit2/issues/21 --- .../include/moveit/robot_state/log.h | 53 +++++++++++ moveit_core/robot_state/src/conversions.cpp | 46 +++++----- moveit_core/robot_state/src/robot_state.cpp | 87 +++++++++---------- 3 files changed, 118 insertions(+), 68 deletions(-) create mode 100644 moveit_core/robot_state/include/moveit/robot_state/log.h diff --git a/moveit_core/robot_state/include/moveit/robot_state/log.h b/moveit_core/robot_state/include/moveit/robot_state/log.h new file mode 100644 index 0000000000..99eabfd87d --- /dev/null +++ b/moveit_core/robot_state/include/moveit/robot_state/log.h @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Acutronic Robotics AG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Víctor Mayoral Vilches */ + +#ifndef MOVEIT_CORE_ROBOT_STATE_LOG_ +#define MOVEIT_CORE_ROBOT_STATE_LOG_ + +#include "rclcpp/rclcpp.hpp" + +namespace moveit +{ +namespace core +{ + +// Logger +rclcpp::Logger logger_robot_state = rclcpp::get_logger("robot_state"); + +}; +}; + +#endif diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 0678e58693..3fe8d4ae4c 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -39,13 +39,12 @@ #include #include #include -#include +#include namespace moveit { namespace core { -const std::string LOGNAME = "robot_state"; // ******************************************** // * Internal (hidden) functions @@ -57,7 +56,7 @@ static bool _jointStateToRobotState(const sensor_msgs::msg::JointState& joint_st { if (joint_state.name.size() != joint_state.position.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Different number of names and positions in JointState message: %zu, %zu", + RCLCPP_ERROR(logger_robot_state, "Different number of names and positions in JointState message: %zu, %zu", joint_state.name.size(), joint_state.position.size()); return false; } @@ -73,7 +72,7 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta std::size_t nj = mjs.joint_names.size(); if (nj != mjs.transforms.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Different number of names, values or frames in MultiDOFJointState message."); + RCLCPP_ERROR(logger_robot_state, "Different number of names, values or frames in MultiDOFJointState message."); return false; } @@ -95,14 +94,14 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta } catch (std::exception& ex) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Caught %s", ex.what()); + RCLCPP_ERROR(logger_robot_state, "Caught %s", ex.what()); error = true; } else error = true; if (error) - ROS_WARN_NAMED(LOGNAME.c_str(), "The transform for multi-dof joints was specified in frame '%s' " + RCLCPP_WARN(logger_robot_state, "The transform for multi-dof joints was specified in frame '%s' " "but it was not possible to transform that to frame '%s'", mjs.header.frame_id.c_str(), state.getRobotModel()->getModelFrame().c_str()); } @@ -112,7 +111,7 @@ static bool _multiDOFJointsToRobotState(const sensor_msgs::msg::MultiDOFJointSta const std::string& joint_name = mjs.joint_names[i]; if (!state.getRobotModel()->hasJointModel(joint_name)) { - ROS_WARN_NAMED(LOGNAME.c_str(), "No joint matching multi-dof joint '%s'", joint_name.c_str()); + RCLCPP_WARN(logger_robot_state, "No joint matching multi-dof joint '%s'", joint_name.c_str()); error = true; continue; } @@ -229,20 +228,20 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att { if (aco.object.primitives.size() != aco.object.primitive_poses.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of primitive shapes does not match " + RCLCPP_ERROR(logger_robot_state, "Number of primitive shapes does not match " "number of poses in collision object message"); return; } if (aco.object.meshes.size() != aco.object.mesh_poses.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of meshes does not match number of poses in collision object message"); + RCLCPP_ERROR(logger_robot_state, "Number of meshes does not match number of poses in collision object message"); return; } if (aco.object.planes.size() != aco.object.plane_poses.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of planes does not match number of poses in collision object message"); + RCLCPP_ERROR(logger_robot_state, "Number of planes does not match number of poses in collision object message"); return; } @@ -298,7 +297,7 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att else { t0.setIdentity(); - ROS_ERROR_NAMED(LOGNAME.c_str(), "Cannot properly transform from frame '%s'. " + RCLCPP_ERROR(logger_robot_state, "Cannot properly transform from frame '%s'. " "The pose of the attached body may be incorrect", aco.object.header.frame_id.c_str()); } @@ -307,31 +306,32 @@ static void _msgToAttachedBody(const Transforms* tf, const moveit_msgs::msg::Att poses[i] = t * poses[i]; } - if (shapes.empty()) - ROS_ERROR_NAMED(LOGNAME.c_str(), "There is no geometry to attach to link '%s' as part of attached body '%s'", - aco.link_name.c_str(), aco.object.id.c_str()); + if (shapes.empty()) { + RCLCPP_ERROR(logger_robot_state, "There is no geometry to attach to link '%s' as part of attached body '%s'", + aco.link_name.c_str(), aco.object.id.c_str()); + } else { if (state.clearAttachedBody(aco.object.id)) - ROS_DEBUG_NAMED(LOGNAME.c_str(), "The robot state already had an object named '%s' attached to link '%s'. " + RCLCPP_DEBUG(logger_robot_state, "The robot state already had an object named '%s' attached to link '%s'. " "The object was replaced.", aco.object.id.c_str(), aco.link_name.c_str()); state.attachBody(aco.object.id, shapes, poses, aco.touch_links, aco.link_name, aco.detach_posture); - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); + RCLCPP_DEBUG(logger_robot_state, "Attached object '%s' to link '%s'", aco.object.id.c_str(), aco.link_name.c_str()); } } } else - ROS_ERROR_NAMED(LOGNAME.c_str(), "The attached body for link '%s' has no geometry", aco.link_name.c_str()); + RCLCPP_ERROR(logger_robot_state, "The attached body for link '%s' has no geometry", aco.link_name.c_str()); } else if (aco.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { if (!state.clearAttachedBody(aco.object.id)) - ROS_ERROR_NAMED(LOGNAME.c_str(), "The attached body '%s' can not be removed because it does not exist", + RCLCPP_ERROR(logger_robot_state, "The attached body '%s' can not be removed because it does not exist", aco.link_name.c_str()); } else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Unknown collision object operation: %d", aco.object.operation); + RCLCPP_ERROR(logger_robot_state, "Unknown collision object operation: %d", aco.object.operation); } static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_msgs::msg::RobotState& robot_state, @@ -342,7 +342,7 @@ static bool _robotStateMsgToRobotStateHelper(const Transforms* tf, const moveit_ if (!rs.is_diff && rs.joint_state.name.empty() && rs.multi_dof_joint_state.joint_names.empty()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Found empty JointState message"); + RCLCPP_ERROR(logger_robot_state, "Found empty JointState message"); return false; } @@ -438,12 +438,12 @@ bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& tra { if (trajectory.points.empty() || point_id > trajectory.points.size() - 1) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Invalid point_id"); + RCLCPP_ERROR(logger_robot_state, "Invalid point_id"); return false; } if (trajectory.joint_names.empty()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "No joint names specified"); + RCLCPP_ERROR(logger_robot_state, "No joint names specified"); return false; } @@ -534,7 +534,7 @@ void streamToRobotState(RobotState& state, const std::string& line, const std::s // Get a variable if (!std::getline(line_stream, cell, separator[0])) // ROS_ERROR_STREAM_NAMED(LOGNAME.c_str(), "Missing variable " << state.getVariableNames()[i]); - ROS_ERROR_NAMED(LOGNAME.c_str(), "Missing variable " , state.getVariableNames()[i].c_str()); + RCLCPP_ERROR(logger_robot_state, "Missing variable " , state.getVariableNames()[i].c_str()); printf("show variablename %s\n", state.getVariableNames()[i].c_str()); state.getVariablePositions()[i] = boost::lexical_cast(cell.c_str()); } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index c5f0aae436..3eb2360621 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -43,8 +43,7 @@ #include #include #include -#include - +#include #include "rclcpp/rclcpp.hpp" namespace moveit @@ -57,8 +56,6 @@ namespace core * valid paths from paths with large joint space jumps. */ static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10; -const std::string LOGNAME = "robot_state"; - RobotState::RobotState(const RobotModelConstPtr& robot_model) : robot_model_(robot_model) , has_velocity_(false) @@ -176,7 +173,7 @@ bool RobotState::checkJointTransforms(const JointModel* joint) const { if (dirtyJointTransform(joint)) { - ROS_WARN_NAMED(LOGNAME.c_str(), "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); + RCLCPP_WARN(logger_robot_state, "Returning dirty joint transforms for joint '%s'", joint->getName().c_str()); return false; } return true; @@ -186,7 +183,7 @@ bool RobotState::checkLinkTransforms() const { if (dirtyLinkTransforms()) { - ROS_WARN_NAMED(LOGNAME.c_str(), "Returning dirty link transforms"); + RCLCPP_WARN(logger_robot_state, "Returning dirty link transforms"); return false; } return true; @@ -196,7 +193,7 @@ bool RobotState::checkCollisionTransforms() const { if (dirtyCollisionBodyTransforms()) { - ROS_WARN_NAMED(LOGNAME.c_str(), "Returning dirty collision body transforms"); + RCLCPP_WARN(logger_robot_state, "Returning dirty collision body transforms"); return false; } return true; @@ -436,7 +433,7 @@ void RobotState::setJointEfforts(const JointModel* joint, const double* effort) { if (has_acceleration_) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Unable to set joint efforts because array is being used for accelerations"); + RCLCPP_ERROR(logger_robot_state, "Unable to set joint efforts because array is being used for accelerations"); return; } has_effort_ = true; @@ -854,7 +851,7 @@ const AttachedBody* RobotState::getAttachedBody(const std::string& id) const std::map::const_iterator it = attached_body_map_.find(id); if (it == attached_body_map_.end()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Attached body '%s' not found", id.c_str()); + RCLCPP_ERROR(logger_robot_state, "Attached body '%s' not found", id.c_str()); return nullptr; } else @@ -995,7 +992,7 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& id) co std::map::const_iterator jt = attached_body_map_.find(id); if (jt == attached_body_map_.end()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Transform from frame '%s' to frame '%s' is not known " + RCLCPP_ERROR(logger_robot_state, "Transform from frame '%s' to frame '%s' is not known " "('%s' should be a link name or an attached body id).", id.c_str(), robot_model_->getModelFrame().c_str(), id.c_str()); return IDENTITY_TRANSFORM; @@ -1003,12 +1000,12 @@ const Eigen::Isometry3d& RobotState::getFrameTransform(const std::string& id) co const EigenSTL::vector_Isometry3d& tf = jt->second->getGlobalCollisionBodyTransforms(); if (tf.empty()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Attached body '%s' has no geometry associated to it. No transform to return.", + RCLCPP_ERROR(logger_robot_state, "Attached body '%s' has no geometry associated to it. No transform to return.", id.c_str()); return IDENTITY_TRANSFORM; } if (tf.size() > 1) - ROS_DEBUG_NAMED(LOGNAME.c_str(), "There are multiple geometries associated to attached body '%s'. " + RCLCPP_DEBUG(logger_robot_state, "There are multiple geometries associated to attached body '%s'. " "Returning the transform for the first one.", id.c_str()); return tf[0]; @@ -1049,7 +1046,7 @@ void RobotState::getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, cons for (std::size_t i = 0; i < link_names.size(); ++i) { - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Trying to get marker for link '%s'", link_names[i].c_str()); + RCLCPP_DEBUG(logger_robot_state, "Trying to get marker for link '%s'", link_names[i].c_str()); const LinkModel* lm = robot_model_->getLinkModel(link_names[i]); if (!lm) continue; @@ -1129,13 +1126,13 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link if (!group->isChain()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "The group '%s' is not a chain. Cannot compute Jacobian.", group->getName().c_str()); return false; } if (!group->isLinkUpdated(link->getName())) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", + RCLCPP_ERROR(logger_robot_state, "Link name '%s' does not exist in the chain '%s' or is not a child for this chain", link->getName().c_str(), group->getName().c_str()); return false; } @@ -1152,7 +1149,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link Eigen::Vector3d point_transform = link_transform * reference_point_position; /* - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Point from reference origin expressed in world coordinates: %f %f %f", + RCLCPP_DEBUG(logger_robot_state, "Point from reference origin expressed in world coordinates: %f %f %f", point_transform.x(), point_transform.y(), point_transform.z()); @@ -1164,11 +1161,11 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link while (link) { /* - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Link: %s, %f %f %f",link_state->getName().c_str(), + RCLCPP_DEBUG(logger_robot_state, "Link: %s, %f %f %f",link_state->getName().c_str(), link_state->getGlobalLinkTransform().translation().x(), link_state->getGlobalLinkTransform().translation().y(), link_state->getGlobalLinkTransform().translation().z()); - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Joint: %s",link_state->getParentJointState()->getName().c_str()); + RCLCPP_DEBUG(logger_robot_state, "Joint: %s",link_state->getParentJointState()->getName().c_str()); */ const JointModel* pjm = link->getParentJointModel(); if (pjm->getVariableCount() > 0) @@ -1206,7 +1203,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link jacobian.block<3, 1>(3, joint_index + 2) = jacobian.block<3, 1>(3, joint_index + 2) + joint_axis; } else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Unknown type of joint in Jacobian computation"); + RCLCPP_ERROR(logger_robot_state, "Unknown type of joint in Jacobian computation"); } if (pjm == root_joint_model) break; @@ -1311,7 +1308,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg: const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } return setFromIK(jmg, pose, solver->getTipFrame(), attempts, timeout, constraint, options); @@ -1334,7 +1331,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); if (!solver) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } static std::vector consistency_limits; @@ -1381,7 +1378,7 @@ bool RobotState::setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& if (!lm) { // ROS_ERROR_STREAM_NAMED(LOGNAME, "IK frame '" << ik_frame << "' does not exist."); - ROS_ERROR_NAMED(LOGNAME.c_str(), "The following IK frame does not exist:", ik_frame.c_str()); + RCLCPP_ERROR(logger_robot_state, "The following IK frame does not exist:", ik_frame.c_str()); return false; } pose = getGlobalLinkTransform(lm).inverse() * pose; @@ -1425,7 +1422,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Error check if (poses_in.size() != tips_in.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of poses must be the same as number of tips"); + RCLCPP_ERROR(logger_robot_state, "Number of poses must be the same as number of tips"); return false; } @@ -1444,7 +1441,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::string error_msg; if (!solver->supportsGroup(jmg, &error_msg)) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Kinematics solver %s does not support joint group %s. Error: %s", + RCLCPP_ERROR(logger_robot_state, "Kinematics solver %s does not support joint group %s. Error: %s", typeid(*solver).name(), jmg->getName().c_str(), error_msg.c_str()); valid_solver = false; } @@ -1460,7 +1457,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } else { - ROS_ERROR_NAMED(LOGNAME.c_str(), "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "No kinematics solver instantiated for group '%s'", jmg->getName().c_str()); return false; } } @@ -1469,7 +1466,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is std::vector consistency_limits; if (consistency_limit_sets.size() > 1) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Invalid number (%zu) of sets of consistency limits for a setFromIK request " + RCLCPP_ERROR(logger_robot_state, "Invalid number (%zu) of sets of consistency limits for a setFromIK request " "that is being solved by a single IK solver", consistency_limit_sets.size()); return false; @@ -1526,7 +1523,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is const EigenSTL::vector_Isometry3d& ab_trans = ab->getFixedTransforms(); if (ab_trans.size() != 1) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Cannot use an attached body " + RCLCPP_ERROR(logger_robot_state, "Cannot use an attached body " "with multiple geometries as a reference frame."); return false; } @@ -1539,7 +1536,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is if (!lm) { // ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist."); - ROS_ERROR_NAMED(LOGNAME.c_str(), "The following Pose Frame does not exist ", pose_frame.c_str()); + RCLCPP_ERROR(logger_robot_state, "The following Pose Frame does not exist ", pose_frame.c_str()); return false; } const robot_model::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms(); @@ -1566,12 +1563,12 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is // Make sure one of the tip frames worked if (!found_valid_frame) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); + RCLCPP_ERROR(logger_robot_state, "Cannot compute IK for query %zu pose reference frame '%s'", i, pose_frame.c_str()); // Debug available tip frames std::stringstream ss; for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id) ss << solver_tip_frames[solver_tip_id] << ", "; - ROS_ERROR_NAMED(LOGNAME.c_str(), "Available tip frames: [%s]", ss.str().c_str()); + RCLCPP_ERROR(logger_robot_state, "Available tip frames: [%s]", ss.str().c_str()); return false; } @@ -1650,7 +1647,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is } else { - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Rerunning IK solver with random joint positions"); + RCLCPP_DEBUG(logger_robot_state, "Rerunning IK solver with random joint positions"); // sample a random seed random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator(); @@ -1701,21 +1698,21 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // Error check if (poses_in.size() != sub_groups.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), + RCLCPP_ERROR(logger_robot_state, "Number of poses (%zu) must be the same as number of sub-groups (%zu)", poses_in.size(), sub_groups.size()); return false; } if (tips_in.size() != sub_groups.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), + RCLCPP_ERROR(logger_robot_state, "Number of tip names (%zu) must be same as number of sub-groups (%zu)", tips_in.size(), sub_groups.size()); return false; } if (!consistency_limits.empty() && consistency_limits.size() != sub_groups.size()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of consistency limit vectors must be the same as number of sub-groups"); + RCLCPP_ERROR(logger_robot_state, "Number of consistency limit vectors must be the same as number of sub-groups"); return false; } @@ -1723,7 +1720,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: { if (consistency_limits[i].size() != sub_groups[i]->getVariableCount()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Number of joints in consistency_limits is %zu but it should be should be %u", i, + RCLCPP_ERROR(logger_robot_state, "Number of joints in consistency_limits is %zu but it should be should be %u", i, sub_groups[i]->getVariableCount()); return false; } @@ -1736,7 +1733,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: kinematics::KinematicsBaseConstPtr solver = sub_groups[i]->getSolverInstance(); if (!solver) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); + RCLCPP_ERROR(logger_robot_state, "Could not find solver for group '%s'", sub_groups[i]->getName().c_str()); return false; } solvers.push_back(solver); @@ -1772,7 +1769,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: const EigenSTL::vector_Isometry3d& ab_trans = ab->getFixedTransforms(); if (ab_trans.size() != 1) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Cannot use an attached body with multiple geometries as a reference frame."); + RCLCPP_ERROR(logger_robot_state, "Cannot use an attached body with multiple geometries as a reference frame."); return false; } pose_frame = ab->getAttachedLinkName(); @@ -1796,7 +1793,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (pose_frame != solver_tip_frame) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Cannot compute IK for query pose reference frame '%s', desired: '%s'", + RCLCPP_ERROR(logger_robot_state, "Cannot compute IK for query pose reference frame '%s', desired: '%s'", pose_frame.c_str(), solver_tip_frame.c_str()); return false; } @@ -1870,7 +1867,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: found_solution = false; break; } - ROS_DEBUG_NAMED(LOGNAME.c_str(), "IK attempt: %d of %d", st, attempts); + RCLCPP_DEBUG(logger_robot_state, "IK attempt: %d of %d", st, attempts); } if (found_solution) { @@ -1878,7 +1875,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: copyJointGroupPositions(jmg, full_solution); if (constraint ? constraint(this, jmg, &full_solution[0]) : true) { - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Found IK solution"); + RCLCPP_DEBUG(logger_robot_state, "Found IK solution"); return true; } } @@ -1931,7 +1928,7 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { - ROS_ERROR_NAMED(LOGNAME.c_str(), + RCLCPP_ERROR(logger_robot_state, "Invalid MaxEEFStep passed into computeCartesianPath. Both the MaxEEFStep.rotation and " "MaxEEFStep.translation components must be non-negative and at least one component must be " "greater than zero"); @@ -2063,7 +2060,7 @@ double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std: { if (traj.size() < MIN_STEPS_FOR_JUMP_THRESH) { - ROS_WARN_NAMED(LOGNAME.c_str(), "The computed trajectory is too short to detect jumps in joint-space " + RCLCPP_WARN(logger_robot_state, "The computed trajectory is too short to detect jumps in joint-space " "Need at least %zu steps, only got %zu. Try a lower max_step.", MIN_STEPS_FOR_JUMP_THRESH, traj.size()); } @@ -2084,7 +2081,7 @@ double RobotState::testRelativeJointSpaceJump(const JointModelGroup* group, std: for (std::size_t i = 0; i < dist_vector.size(); ++i) if (dist_vector[i] > thres) { - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Truncating Cartesian path due to detected jump in joint-space distance"); + RCLCPP_DEBUG(logger_robot_state, "Truncating Cartesian path due to detected jump in joint-space distance"); percentage = (double)(i + 1) / (double)(traj.size()); traj.resize(i + 1); break; @@ -2119,7 +2116,7 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: type_index = 1; break; default: - ROS_WARN_NAMED(LOGNAME.c_str(), "Joint %s has not supported type %s. \n" + RCLCPP_WARN(logger_robot_state, "Joint %s has not supported type %s. \n" "testAbsoluteJointSpaceJump only supports prismatic and revolute joints.", joint->getName().c_str(), joint->getTypeName().c_str()); continue; @@ -2130,7 +2127,7 @@ double RobotState::testAbsoluteJointSpaceJump(const JointModelGroup* group, std: double distance = traj[traj_ix]->distance(*traj[traj_ix + 1], joint); if (distance > data[type_index].limit_) { - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, + RCLCPP_DEBUG(logger_robot_state, "Truncating Cartesian path due to detected jump of %.4f > %.4f in joint %s", distance, data[type_index].limit_, joint->getName().c_str()); still_valid = false; break;