Skip to content

Commit

Permalink
robot_state, fix logging
Browse files Browse the repository at this point in the history 
Adapt code to new logging system as discussed
at moveit#21
  • Loading branch information
@vmayoral
vmayoral committed Mar 5, 2019
1 parent c2974e0 commit 85ca81d
Show file tree
Hide file tree
Showing 3 changed files with 118 additions and 68 deletions.
53 changes: 53 additions & 0 deletions moveit_core/robot_state/include/moveit/robot_state/log.h
View file
Original file line number Diff line number Diff line change
@@ -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
46 changes: 23 additions & 23 deletions moveit_core/robot_state/src/conversions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,12 @@
#include <geometric_shapes/shape_operations.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/lexical_cast.hpp>
#include <moveit/logging/logging.h>
#include <moveit/robot_state/log.h>

namespace moveit
{
namespace core
{
const std::string LOGNAME = "robot_state";

// ********************************************
// * Internal (hidden) functions
Expand All @@ -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;
}
Expand All @@ -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;
}

Expand All @@ -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());
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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());
}
Expand All @@ -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,
Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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<double>(cell.c_str());
}
Expand Down
Loading

0 comments on commit 85ca81d

Please sign in to comment.