From 5db92184623889c4e45724e0eff8f81a34f9e6f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Tue, 19 Feb 2019 10:10:00 +0100 Subject: [PATCH 01/12] Port robot_model submodule of moveit_core to ROS 2 --- moveit_core/robot_model/CMakeLists.txt | 30 ++++++-- .../include/moveit/robot_model/joint_model.h | 13 +++- .../include/moveit/robot_model/robot_model.h | 2 +- .../robot_model/src/floating_joint_model.cpp | 2 +- moveit_core/robot_model/src/joint_model.cpp | 4 +- moveit_core/robot_model/src/robot_model.cpp | 74 +++++++++---------- 6 files changed, 72 insertions(+), 53 deletions(-) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index a3383680b3..33dcf12c5e 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -1,5 +1,15 @@ +cmake_minimum_required(VERSION 3.5) set(MOVEIT_LIB_NAME moveit_robot_model) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + add_library(${MOVEIT_LIB_NAME} src/aabb.cpp src/fixed_joint_model.cpp @@ -12,20 +22,24 @@ add_library(${MOVEIT_LIB_NAME} src/revolute_joint_model.cpp src/robot_model.cpp ) -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) -target_link_libraries(${MOVEIT_LIB_NAME} moveit_profiler moveit_exceptions moveit_kinematics_base ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${MOVEIT_LIB_NAME} moveit_profiler moveit_exceptions moveit_kinematics_base ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) +# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(${MOVEIT_LIB_NAME} + urdf + urdfdom_headers + visualization_msgs) -if(CATKIN_ENABLE_TESTING) +if(AMENT_ENABLE_TESTING) find_package(moveit_resources REQUIRED) include_directories(${moveit_resources_INCLUDE_DIRS}) catkin_add_gtest(test_robot_model test/test.cpp) - target_link_libraries(test_robot_model moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) + target_link_libraries(test_robot_model moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) endif() install(TARGETS ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 982ae78bb8..39af629658 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -42,9 +42,14 @@ #include #include #include -#include +// #include +#include #include #include +#include + +#define ROS_DEBUG_NAMED RCUTILS_LOG_ERROR +#define ROS_WARN_NAMED RCUTILS_LOG_ERROR namespace moveit { @@ -336,10 +341,10 @@ class JointModel void setVariableBounds(const std::string& variable, const VariableBounds& bounds); /** \brief Override joint limits loaded from URDF. Unknown variables are ignored. */ - void setVariableBounds(const std::vector& jlim); + void setVariableBounds(const std::vector& jlim); /** \brief Get the joint limits known to this model, as a message. */ - const std::vector& getVariableBoundsMsg() const + const std::vector& getVariableBoundsMsg() const { return variable_bounds_msg_; } @@ -470,7 +475,7 @@ class JointModel /** \brief The bounds for each variable (low, high) in the same order as variable_names_ */ Bounds variable_bounds_; - std::vector variable_bounds_msg_; + std::vector variable_bounds_msg_; /** \brief Map from variable names to the corresponding index in variable_names_ (indexing makes sense within the * JointModel only) */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index fb09a22a7c..4e354acec4 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -40,7 +40,6 @@ #include #include -#include #include // joint types @@ -53,6 +52,7 @@ #include #include +#include /** \brief Main namespace for MoveIt! */ namespace moveit diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 47ed6a87d2..55ddd4364f 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -37,7 +37,7 @@ #include #include -#include +// #include #include #include diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index 49d824a738..286b379b41 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -133,7 +133,7 @@ void JointModel::setVariableBounds(const std::string& variable, const VariableBo computeVariableBoundsMsg(); } -void JointModel::setVariableBounds(const std::vector& jlim) +void JointModel::setVariableBounds(const std::vector& jlim) { for (std::size_t j = 0; j < variable_names_.size(); ++j) for (std::size_t i = 0; i < jlim.size(); ++i) @@ -167,7 +167,7 @@ void JointModel::computeVariableBoundsMsg() variable_bounds_msg_.clear(); for (std::size_t i = 0; i < variable_bounds_.size(); ++i) { - moveit_msgs::JointLimits lim; + moveit_msgs::msg::JointLimits lim; lim.joint_name = variable_names_[i]; lim.has_position_limits = variable_bounds_[i].position_bounded_; lim.min_position = variable_bounds_[i].min_position_; diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index b4fbee8e78..a071dde952 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -90,39 +90,39 @@ void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf:: link_geometry_count_ = 0; variable_count_ = 0; model_name_ = urdf_model.getName(); - ROS_INFO_NAMED(LOGNAME, "Loading robot model '%s'...", model_name_.c_str()); + ROS_INFO_NAMED(LOGNAME.c_str(), "Loading robot model '%s'...", model_name_.c_str()); if (urdf_model.getRoot()) { const urdf::Link* root_link_ptr = urdf_model.getRoot().get(); model_frame_ = root_link_ptr->name; - ROS_DEBUG_NAMED(LOGNAME, "... building kinematic chain"); + ROS_DEBUG_NAMED(LOGNAME.c_str(), "... building kinematic chain"); root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model); if (root_joint_) root_link_ = root_joint_->getChildLinkModel(); - ROS_DEBUG_NAMED(LOGNAME, "... building mimic joints"); + ROS_DEBUG_NAMED(LOGNAME.c_str(), "... building mimic joints"); buildMimic(urdf_model); - ROS_DEBUG_NAMED(LOGNAME, "... computing joint indexing"); + ROS_DEBUG_NAMED(LOGNAME.c_str(), "... computing joint indexing"); buildJointInfo(); if (link_models_with_collision_geometry_vector_.empty()) - ROS_WARN_NAMED(LOGNAME, "No geometry is associated to any robot links"); + ROS_WARN_NAMED(LOGNAME.c_str(), "No geometry is associated to any robot links"); // build groups - ROS_DEBUG_NAMED(LOGNAME, "... constructing joint groups"); + ROS_DEBUG_NAMED(LOGNAME.c_str(), "... constructing joint groups"); buildGroups(srdf_model); - ROS_DEBUG_NAMED(LOGNAME, "... constructing joint group states"); + ROS_DEBUG_NAMED(LOGNAME.c_str(), "... constructing joint group states"); buildGroupStates(srdf_model); // For debugging entire model // printModelInfo(std::cout); } else - ROS_WARN_NAMED(LOGNAME, "No root link found"); + ROS_WARN_NAMED(LOGNAME.c_str(), "No root link found"); } namespace @@ -340,13 +340,13 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) for (std::size_t j = 0; j < vn.size(); ++j) state[vn[j]] = jt->second[j]; else - ROS_ERROR_NAMED(LOGNAME, "The model for joint '%s' requires %d variable values, " + ROS_ERROR_NAMED(LOGNAME.c_str(), "The model for joint '%s' requires %d variable values, " "but only %d variable values were supplied in default state '%s' for group '%s'", jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), jmg->getName().c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specifies value for joint '%s', " + ROS_ERROR_NAMED(LOGNAME.c_str(), "Group state '%s' specifies value for joint '%s', " "but that joint is not part of group '%s'", ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } @@ -354,7 +354,7 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) jmg->addDefaultState(ds[i].name_, state); } else - ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specified for group '%s', but that group does not exist", + ROS_ERROR_NAMED(LOGNAME.c_str(), "Group state '%s' specified for group '%s', but that group does not exist", ds[i].name_.c_str(), ds[i].group_.c_str()); } } @@ -374,11 +374,11 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount()) joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset); else - ROS_ERROR_NAMED(LOGNAME, "Join '%s' cannot mimic joint '%s' because they have different number of DOF", + ROS_ERROR_NAMED(LOGNAME.c_str(), "Join '%s' cannot mimic joint '%s' because they have different number of DOF", joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Joint '%s' cannot mimic unknown joint '%s'", + ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint '%s' cannot mimic unknown joint '%s'", joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); } } @@ -402,7 +402,7 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) } if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic()) { - ROS_ERROR_NAMED(LOGNAME, "Cycle found in joint that mimic each other. Ignoring all mimic joints."); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Cycle found in joint that mimic each other. Ignoring all mimic joints."); for (std::size_t i = 0; i < joint_model_vector_.size(); ++i) joint_model_vector_[i]->setMimic(nullptr, 0.0, 0.0); change = false; @@ -432,7 +432,7 @@ const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -446,7 +446,7 @@ JointModelGroup* RobotModel::getEndEffector(const std::string& name) it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -462,7 +462,7 @@ const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) c JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -473,7 +473,7 @@ JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -508,14 +508,14 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) added = true; processed[i] = true; if (!addJointModelGroup(group_configs[i])) - ROS_WARN_NAMED(LOGNAME, "Failed to add group '%s'", group_configs[i].name_.c_str()); + ROS_WARN_NAMED(LOGNAME.c_str(), "Failed to add group '%s'", group_configs[i].name_.c_str()); } } } for (std::size_t i = 0; i < processed.size(); ++i) if (!processed[i]) - ROS_WARN_NAMED(LOGNAME, "Could not process group '%s' due to unmet subgroup dependencies", + ROS_WARN_NAMED(LOGNAME.c_str(), "Could not process group '%s' due to unmet subgroup dependencies", group_configs[i].name_.c_str()); for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it) @@ -601,16 +601,16 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) if (jt->second != it->second) eef_parent_group = jt->second; else - ROS_ERROR_NAMED(LOGNAME, "Group '%s' for end-effector '%s' cannot be its own parent", + ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' for end-effector '%s' cannot be its own parent", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Group '%s' was specified as parent group for end-effector '%s' " + ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' was specified as parent group for end-effector '%s' " "but it does not include the parent link '%s'", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str()); } else - ROS_ERROR_NAMED(LOGNAME, "Group name '%s' not found (specified as parent group for end-effector '%s')", + ROS_ERROR_NAMED(LOGNAME.c_str(), "Group name '%s' not found (specified as parent group for end-effector '%s')", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); } @@ -634,7 +634,7 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) } else { - ROS_WARN_NAMED(LOGNAME, "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str()); + ROS_WARN_NAMED(LOGNAME.c_str(), "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str()); it->second->setEndEffectorParent("", eefs[k].parent_link_); } } @@ -646,7 +646,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) { if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end()) { - ROS_WARN_NAMED(LOGNAME, "A group named '%s' already exists. Not adding.", gc.name_.c_str()); + ROS_WARN_NAMED(LOGNAME.c_str(), "A group named '%s' already exists. Not adding.", gc.name_.c_str()); return false; } @@ -743,7 +743,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) if (jset.empty()) { - ROS_WARN_NAMED(LOGNAME, "Group '%s' must have at least one valid joint", gc.name_.c_str()); + ROS_WARN_NAMED(LOGNAME.c_str(), "Group '%s' must have at least one valid joint", gc.name_.c_str()); return false; } @@ -888,7 +888,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const result = new FixedJointModel(urdf_joint->name); break; default: - ROS_ERROR_NAMED(LOGNAME, "Unknown joint type: %d", (int)urdf_joint->type); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Unknown joint type: %d", (int)urdf_joint->type); break; } } @@ -899,14 +899,14 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const { if (virtual_joints[i].child_link_ != child_link->name) { - ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its child frame '%s' " + ROS_WARN_NAMED(LOGNAME.c_str(), "Skipping virtual joint '%s' because its child frame '%s' " "does not match the URDF frame '%s'", virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(), child_link->name.c_str()); } else if (virtual_joints[i].parent_frame_.empty()) { - ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its parent frame is empty", + ROS_WARN_NAMED(LOGNAME.c_str(), "Skipping virtual joint '%s' because its parent frame is empty", virtual_joints[i].name_.c_str()); } else @@ -930,7 +930,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const } if (!result) { - ROS_INFO_NAMED(LOGNAME, "No root/virtual joint specified in SRDF. Assuming fixed joint"); + ROS_INFO_NAMED(LOGNAME.c_str(), "No root/virtual joint specified in SRDF. Assuming fixed joint"); result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); } } @@ -1062,7 +1062,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) } break; default: - ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)geom->type); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Unknown geometry type: %d", (int)geom->type); break; } @@ -1084,7 +1084,7 @@ const JointModel* RobotModel::getJointModel(const std::string& name) const JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1092,7 +1092,7 @@ const JointModel* RobotModel::getJointModel(int index) const { if (index < 0 || index >= static_cast(joint_model_vector_.size())) { - ROS_ERROR_NAMED(LOGNAME, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); return nullptr; } assert(joint_model_vector_[index]->getJointIndex() == index); @@ -1104,7 +1104,7 @@ JointModel* RobotModel::getJointModel(const std::string& name) JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1117,7 +1117,7 @@ const LinkModel* RobotModel::getLinkModel(int index) const { if (index < 0 || index >= static_cast(link_model_vector_.size())) { - ROS_ERROR_NAMED(LOGNAME, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); return nullptr; } assert(link_model_vector_[index]->getLinkIndex() == index); @@ -1129,7 +1129,7 @@ LinkModel* RobotModel::getLinkModel(const std::string& name) LinkModelMap::const_iterator it = link_model_map_.find(name); if (it != link_model_map_.end()) return it->second; - ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + ROS_ERROR_NAMED(LOGNAME.c_str(), "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1330,7 +1330,7 @@ void RobotModel::setKinematicsAllocators(const std::mapgetName() << " "; result.second[subs[i]] = allocators.find(subs[i]->getName())->second; } - ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), + ROS_DEBUG_NAMED(LOGNAME.c_str(), "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str()); } jmg->setSolverAllocators(result); From 18edf88b3e85f2c377e0618b3105ac55d0a5ee1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 24 Feb 2019 13:23:13 +0100 Subject: [PATCH 02/12] Clean submodule CMakeLists.txt Remove unnecesary commands listed already one level higher --- moveit_core/robot_model/CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 33dcf12c5e..725a1f133f 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -1,15 +1,6 @@ cmake_minimum_required(VERSION 3.5) set(MOVEIT_LIB_NAME moveit_robot_model) -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - add_library(${MOVEIT_LIB_NAME} src/aabb.cpp src/fixed_joint_model.cpp From db9f036179f24131217477fc0da50b5cb35a0263 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 24 Feb 2019 13:34:13 +0100 Subject: [PATCH 03/12] robot_model, simplify CMakeLists.txt Following guidelines provided at https://github.com/ros-planning/moveit2/pull/8#discussion_r258720977 --- moveit_core/robot_model/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 725a1f133f..1acfda8b37 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -13,14 +13,14 @@ add_library(${MOVEIT_LIB_NAME} src/revolute_joint_model.cpp src/robot_model.cpp ) -# set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) -target_link_libraries(${MOVEIT_LIB_NAME} moveit_profiler moveit_exceptions moveit_kinematics_base ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) -# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom_headers - visualization_msgs) + visualization_msgs + moveit_profiler + moveit_exceptions + moveit_kinematics_base) if(AMENT_ENABLE_TESTING) find_package(moveit_resources REQUIRED) From 700c45c2fcb6bbab76ce2eb745cf49dec4e3b04b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 24 Feb 2019 13:39:46 +0100 Subject: [PATCH 04/12] Fix testing and comment it out for now moveit_resources hasn't been ported just yet --- moveit_core/robot_model/CMakeLists.txt | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 1acfda8b37..dae9ca2e9a 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -22,13 +22,14 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_exceptions moveit_kinematics_base) -if(AMENT_ENABLE_TESTING) - find_package(moveit_resources REQUIRED) - include_directories(${moveit_resources_INCLUDE_DIRS}) - - catkin_add_gtest(test_robot_model test/test.cpp) - target_link_libraries(test_robot_model moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) -endif() +# if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# find_package(moveit_resources REQUIRED) +# include_directories(${moveit_resources_INCLUDE_DIRS}) +# +# ament_add_gtest(test_robot_model test/test.cpp) +# target_link_libraries(test_robot_model moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) +# endif() install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib From bbda06334dbfc9c14f327abaae0cbb7f36ba2e29 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 24 Feb 2019 13:42:45 +0100 Subject: [PATCH 05/12] robot_model, cleanup code --- moveit_core/robot_model/include/moveit/robot_model/joint_model.h | 1 - moveit_core/robot_model/src/floating_joint_model.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 39af629658..9c36c4497e 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -42,7 +42,6 @@ #include #include #include -// #include #include #include #include diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 55ddd4364f..7b5c04277b 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -37,7 +37,6 @@ #include #include -// #include #include #include From 95ca014fbcb3ea2c03771013372f29f96b07c738 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Tue, 26 Feb 2019 11:38:24 +0100 Subject: [PATCH 06/12] Revert changes, reenable properties version --- moveit_core/robot_model/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index dae9ca2e9a..b83d01b848 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -13,6 +13,7 @@ add_library(${MOVEIT_LIB_NAME} src/revolute_joint_model.cpp src/robot_model.cpp ) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(${MOVEIT_LIB_NAME} urdf From 70735ccb5878c8613736eb9c32c8d3601b8e1b4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Tue, 26 Feb 2019 21:54:54 +0100 Subject: [PATCH 07/12] robot_model re-enable tests with moveit_resources ported https://github.com/ros-planning/moveit_resources/pull/24 this PR re-enables the testing while allowing the submodule to still compile --- moveit_core/robot_model/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index b83d01b848..da17110ab7 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -23,14 +23,14 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_exceptions moveit_kinematics_base) -# if(BUILD_TESTING) -# find_package(ament_cmake_gtest REQUIRED) -# find_package(moveit_resources REQUIRED) -# include_directories(${moveit_resources_INCLUDE_DIRS}) -# -# ament_add_gtest(test_robot_model test/test.cpp) -# target_link_libraries(test_robot_model moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) -# endif() +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(moveit_resources REQUIRED) + include_directories(${moveit_resources_INCLUDE_DIRS}) + + ament_add_gtest(test_robot_model test/test.cpp) + target_link_libraries(test_robot_model moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) +endif() install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib From 5fa885c2e40e0d4132d9fce7fd3df73f3c25bdc2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Tue, 5 Mar 2019 16:07:31 +0100 Subject: [PATCH 08/12] robot_model submodule, fix logging --- .../include/moveit/robot_model/joint_model.h | 4 +- .../include/moveit/robot_model/log.h | 54 ++++++++ .../include/moveit/robot_model/robot_model.h | 3 +- .../robot_model/src/floating_joint_model.cpp | 2 +- .../robot_model/src/joint_model_group.cpp | 41 +++--- moveit_core/robot_model/src/robot_model.cpp | 130 ++++++++++-------- 6 files changed, 151 insertions(+), 83 deletions(-) create mode 100644 moveit_core/robot_model/include/moveit/robot_model/log.h diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 9c36c4497e..e2367077bd 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -46,9 +46,7 @@ #include #include #include - -#define ROS_DEBUG_NAMED RCUTILS_LOG_ERROR -#define ROS_WARN_NAMED RCUTILS_LOG_ERROR +#include namespace moveit { diff --git a/moveit_core/robot_model/include/moveit/robot_model/log.h b/moveit_core/robot_model/include/moveit/robot_model/log.h new file mode 100644 index 0000000000..4f03a9f734 --- /dev/null +++ b/moveit_core/robot_model/include/moveit/robot_model/log.h @@ -0,0 +1,54 @@ +/********************************************************************* + * 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_MODEL_LOG_ +#define MOVEIT_CORE_ROBOT_MODEL_LOG_ + +#include "rclcpp/rclcpp.hpp" + +namespace moveit +{ +namespace core +{ + +// Logger +rclcpp::Logger logger_robot_model = rclcpp::get_logger("robot_model"); +rclcpp::Logger logger_robot_model_jmg = rclcpp::get_logger("robot_model.jmg"); + +}; +}; + +#endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 4e354acec4..10de8b48ef 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -49,10 +49,9 @@ #include #include #include - +#include #include #include -#include /** \brief Main namespace for MoveIt! */ namespace moveit diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 7b5c04277b..71e623279a 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -170,7 +170,7 @@ bool FloatingJointModel::normalizeRotation(double* values) const double norm = sqrt(norm_sqr); if (norm < std::numeric_limits::epsilon() * 100.0) { - ROS_WARN_NAMED("robot_model", "Quaternion is zero in RobotState representation. Setting to identity"); + RCLCPP_WARN(logger_robot_model, "Quaternion is zero in RobotState representation. Setting to identity"); values[3] = 0.0; values[4] = 0.0; values[5] = 0.0; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 533885cfd6..986447c6f8 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -292,7 +292,7 @@ const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const LinkModelMapConst::const_iterator it = link_model_map_.find(name); if (it == link_model_map_.end()) { - ROS_ERROR_NAMED("robot_model.jmg", "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); + RCLCPP_ERROR(logger_robot_model_jmg, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -303,7 +303,7 @@ const JointModel* JointModelGroup::getJointModel(const std::string& name) const JointModelMapConst::const_iterator it = joint_model_map_.find(name); if (it == joint_model_map_.end()) { - ROS_ERROR_NAMED("robot_model.jmg", "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); + RCLCPP_ERROR(logger_robot_model_jmg, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -344,9 +344,10 @@ void JointModelGroup::getVariableRandomPositionsNearBy( distance_map.find(active_joint_model_vector_[i]->getType()); if (iter != distance_map.end()) distance = iter->second; - else - ROS_WARN_NAMED("robot_model.jmg", "Did not pass in distance for '%s'", - active_joint_model_vector_[i]->getName().c_str()); + else { + RCLCPP_WARN(logger_robot_model_jmg, "Did not pass in distance for '%s'", + active_joint_model_vector_[i]->getName().c_str()); + } active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], near + active_joint_model_start_index_[i], distance); @@ -502,7 +503,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const JointModelGroup* eef = parent_model_->getEndEffector(name); if (!eef) { - ROS_ERROR_NAMED("robot_model.jmg", "Unable to find joint model group for eef"); + RCLCPP_ERROR(logger_robot_model_jmg, "Unable to find joint model group for eef"); return false; } const std::string& eef_parent = eef->getEndEffectorParentGroup().second; @@ -510,7 +511,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent); if (!eef_link) { - ROS_ERROR_NAMED("robot_model.jmg", "Unable to find end effector link for eef"); + RCLCPP_ERROR(logger_robot_model_jmg, "Unable to find end effector link for eef"); return false; } // insert eef_link into tips, maintaining a *sorted* vector, thus enabling use of std::lower_bound @@ -527,11 +528,12 @@ const LinkModel* JointModelGroup::getOnlyOneEndEffectorTip() const getEndEffectorTips(tips); if (tips.size() == 1) return tips.front(); - else if (tips.size() > 1) - ROS_ERROR_NAMED("robot_model.jmg", "More than one end effector tip found for joint model group, " - "so cannot return only one"); - else - ROS_ERROR_NAMED("robot_model.jmg", "No end effector tips found in joint model group"); + else if (tips.size() > 1) { + RCLCPP_ERROR(logger_robot_model_jmg, "More than one end effector tip found for joint model group, so cannot return only one"); + } + else { + RCLCPP_ERROR(logger_robot_model_jmg, "No end effector tips found in joint model group"); + } return nullptr; } @@ -540,7 +542,7 @@ int JointModelGroup::getVariableGroupIndex(const std::string& variable) const VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable); if (it == joint_variables_index_map_.end()) { - ROS_ERROR_NAMED("robot_model.jmg", "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); + RCLCPP_ERROR(logger_robot_model_jmg, "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); return -1; } return it->second; @@ -554,6 +556,13 @@ void JointModelGroup::setDefaultIKTimeout(double ik_timeout) for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it) it->second.default_ik_timeout_ = ik_timeout; } +// Re-added this as its needed to compile robot_state, this has been removed on https://github.com/ros-planning/moveit/pull/1288/files +void JointModelGroup::setDefaultIKAttempts(unsigned int ik_attempts) +{ + group_kinematics_.first.default_ik_attempts_ = ik_attempts; + for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it) + it->second.default_ik_attempts_ = ik_attempts; +} bool JointModelGroup::computeIKIndexBijection(const std::vector& ik_jnames, std::vector& joint_bijection) const @@ -567,7 +576,7 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik // skip reported fixed joints if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED) continue; - ROS_ERROR_NAMED("robot_model.jmg", "IK solver computes joint values for joint '%s' " + RCLCPP_ERROR(logger_robot_model_jmg, "IK solver computes joint values for joint '%s' " "but group '%s' does not contain such a joint.", ik_jnames[i].c_str(), getName().c_str()); return false; @@ -620,7 +629,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const if (tip_frames.empty()) { - ROS_DEBUG_NAMED("robot_model.jmg", "Group %s has no tip frame(s)", name_.c_str()); + RCLCPP_WARN(logger_robot_model_jmg, "Group %s has no tip frame(s)", name_.c_str()); return false; } @@ -630,7 +639,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const // remove frame reference, if specified const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip; const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i]; - ROS_DEBUG_NAMED("robot_model.jmg", "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), + RCLCPP_WARN(logger_robot_model_jmg, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), tip_frame_local.c_str()); // Check if the IK solver's tip is the same as the frame of inquiry diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index a071dde952..347fc8c5c7 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -90,39 +90,41 @@ void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf:: link_geometry_count_ = 0; variable_count_ = 0; model_name_ = urdf_model.getName(); - ROS_INFO_NAMED(LOGNAME.c_str(), "Loading robot model '%s'...", model_name_.c_str()); + RCLCPP_INFO(logger_robot_model, "Loading robot model '%s'...", model_name_.c_str()); if (urdf_model.getRoot()) { const urdf::Link* root_link_ptr = urdf_model.getRoot().get(); model_frame_ = root_link_ptr->name; - ROS_DEBUG_NAMED(LOGNAME.c_str(), "... building kinematic chain"); + RCLCPP_DEBUG(logger_robot_model, "... building kinematic chain"); root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model); if (root_joint_) root_link_ = root_joint_->getChildLinkModel(); - ROS_DEBUG_NAMED(LOGNAME.c_str(), "... building mimic joints"); + RCLCPP_DEBUG(logger_robot_model, "... building mimic joints"); buildMimic(urdf_model); - ROS_DEBUG_NAMED(LOGNAME.c_str(), "... computing joint indexing"); + RCLCPP_DEBUG(logger_robot_model, "... computing joint indexing"); buildJointInfo(); - if (link_models_with_collision_geometry_vector_.empty()) - ROS_WARN_NAMED(LOGNAME.c_str(), "No geometry is associated to any robot links"); + if (link_models_with_collision_geometry_vector_.empty()) { + RCLCPP_WARN(logger_robot_model, "No geometry is associated to any robot links"); + } // build groups - ROS_DEBUG_NAMED(LOGNAME.c_str(), "... constructing joint groups"); + RCLCPP_DEBUG(logger_robot_model, "... constructing joint groups"); buildGroups(srdf_model); - ROS_DEBUG_NAMED(LOGNAME.c_str(), "... constructing joint group states"); + RCLCPP_DEBUG(logger_robot_model, "... constructing joint group states"); buildGroupStates(srdf_model); // For debugging entire model // printModelInfo(std::cout); } - else - ROS_WARN_NAMED(LOGNAME.c_str(), "No root link found"); + else { + RCLCPP_WARN(logger_robot_model, "No root link found"); + } } namespace @@ -339,23 +341,24 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) if (vn.size() == jt->second.size()) for (std::size_t j = 0; j < vn.size(); ++j) state[vn[j]] = jt->second[j]; - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "The model for joint '%s' requires %d variable values, " - "but only %d variable values were supplied in default state '%s' for group '%s'", - jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), - jmg->getName().c_str()); + else { + RCLCPP_ERROR(logger_robot_model, "The model for joint '%s' requires %d variable values, but only %d variable values were supplied in default state '%s' for group '%s'", + jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), + jmg->getName().c_str()); + } + } + else { + RCLCPP_ERROR(logger_robot_model, "Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'", + ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Group state '%s' specifies value for joint '%s', " - "but that joint is not part of group '%s'", - ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } if (!state.empty()) jmg->addDefaultState(ds[i].name_, state); } - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Group state '%s' specified for group '%s', but that group does not exist", + else { + RCLCPP_ERROR(logger_robot_model, "Group state '%s' specified for group '%s', but that group does not exist", ds[i].name_.c_str(), ds[i].group_.c_str()); + } } } @@ -373,13 +376,15 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) { if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount()) joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset); - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Join '%s' cannot mimic joint '%s' because they have different number of DOF", - joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); + else { + RCLCPP_ERROR(logger_robot_model, "Join '%s' cannot mimic joint '%s' because they have different number of DOF", + joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); + } + } + else { + RCLCPP_ERROR(logger_robot_model, "Joint '%s' cannot mimic unknown joint '%s'", + joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); } - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint '%s' cannot mimic unknown joint '%s'", - joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); } } @@ -402,7 +407,7 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) } if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Cycle found in joint that mimic each other. Ignoring all mimic joints."); + RCLCPP_ERROR(logger_robot_model, "Cycle found in joint that mimic each other. Ignoring all mimic joints."); for (std::size_t i = 0; i < joint_model_vector_.size(); ++i) joint_model_vector_[i]->setMimic(nullptr, 0.0, 0.0); change = false; @@ -432,7 +437,7 @@ const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - ROS_ERROR_NAMED(LOGNAME.c_str(), "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -446,7 +451,7 @@ JointModelGroup* RobotModel::getEndEffector(const std::string& name) it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - ROS_ERROR_NAMED(LOGNAME.c_str(), "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -462,7 +467,7 @@ const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) c JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -473,7 +478,7 @@ JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -507,16 +512,18 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) { added = true; processed[i] = true; - if (!addJointModelGroup(group_configs[i])) - ROS_WARN_NAMED(LOGNAME.c_str(), "Failed to add group '%s'", group_configs[i].name_.c_str()); + if (!addJointModelGroup(group_configs[i])) { + RCLCPP_WARN(logger_robot_model, "Failed to add group '%s'", group_configs[i].name_.c_str()); + } } } } for (std::size_t i = 0; i < processed.size(); ++i) - if (!processed[i]) - ROS_WARN_NAMED(LOGNAME.c_str(), "Could not process group '%s' due to unmet subgroup dependencies", - group_configs[i].name_.c_str()); + if (!processed[i]) { + RCLCPP_WARN(logger_robot_model, "Could not process group '%s' due to unmet subgroup dependencies", + group_configs[i].name_.c_str()); + } for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it) joint_model_groups_.push_back(it->second); @@ -600,18 +607,20 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) { if (jt->second != it->second) eef_parent_group = jt->second; - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' for end-effector '%s' cannot be its own parent", - eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); + else { + RCLCPP_ERROR(logger_robot_model, "Group '%s' for end-effector '%s' cannot be its own parent", + eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); + } + } + else { + RCLCPP_ERROR(logger_robot_model, "Group '%s' was specified as parent group for end-effector '%s' but it does not include the parent link '%s'", + eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str()); } - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Group '%s' was specified as parent group for end-effector '%s' " - "but it does not include the parent link '%s'", - eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str()); } - else - ROS_ERROR_NAMED(LOGNAME.c_str(), "Group name '%s' not found (specified as parent group for end-effector '%s')", + else { + RCLCPP_ERROR(logger_robot_model, "Group name '%s' not found (specified as parent group for end-effector '%s')", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); + } } // if no parent group was specified, use a default one @@ -634,7 +643,7 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) } else { - ROS_WARN_NAMED(LOGNAME.c_str(), "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str()); + RCLCPP_WARN(logger_robot_model, "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str()); it->second->setEndEffectorParent("", eefs[k].parent_link_); } } @@ -646,7 +655,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) { if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end()) { - ROS_WARN_NAMED(LOGNAME.c_str(), "A group named '%s' already exists. Not adding.", gc.name_.c_str()); + RCLCPP_WARN(logger_robot_model, "A group named '%s' already exists. Not adding.", gc.name_.c_str()); return false; } @@ -743,7 +752,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) if (jset.empty()) { - ROS_WARN_NAMED(LOGNAME.c_str(), "Group '%s' must have at least one valid joint", gc.name_.c_str()); + RCLCPP_WARN(logger_robot_model, "Group '%s' must have at least one valid joint", gc.name_.c_str()); return false; } @@ -888,7 +897,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const result = new FixedJointModel(urdf_joint->name); break; default: - ROS_ERROR_NAMED(LOGNAME.c_str(), "Unknown joint type: %d", (int)urdf_joint->type); + RCLCPP_ERROR(logger_robot_model, "Unknown joint type: %d", (int)urdf_joint->type); break; } } @@ -899,14 +908,13 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const { if (virtual_joints[i].child_link_ != child_link->name) { - ROS_WARN_NAMED(LOGNAME.c_str(), "Skipping virtual joint '%s' because its child frame '%s' " - "does not match the URDF frame '%s'", + RCLCPP_WARN(logger_robot_model, "Skipping virtual joint '%s' because its child frame '%s' does not match the URDF frame '%s'", virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(), child_link->name.c_str()); } else if (virtual_joints[i].parent_frame_.empty()) { - ROS_WARN_NAMED(LOGNAME.c_str(), "Skipping virtual joint '%s' because its parent frame is empty", + RCLCPP_WARN(logger_robot_model, "Skipping virtual joint '%s' because its parent frame is empty", virtual_joints[i].name_.c_str()); } else @@ -930,7 +938,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const } if (!result) { - ROS_INFO_NAMED(LOGNAME.c_str(), "No root/virtual joint specified in SRDF. Assuming fixed joint"); + RCLCPP_INFO(logger_robot_model, "No root/virtual joint specified in SRDF. Assuming fixed joint"); result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); } } @@ -1062,7 +1070,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) } break; default: - ROS_ERROR_NAMED(LOGNAME.c_str(), "Unknown geometry type: %d", (int)geom->type); + RCLCPP_ERROR(logger_robot_model, "Unknown geometry type: %d", (int)geom->type); break; } @@ -1084,7 +1092,7 @@ const JointModel* RobotModel::getJointModel(const std::string& name) const JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1092,7 +1100,7 @@ const JointModel* RobotModel::getJointModel(int index) const { if (index < 0 || index >= static_cast(joint_model_vector_.size())) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); return nullptr; } assert(joint_model_vector_[index]->getJointIndex() == index); @@ -1104,7 +1112,7 @@ JointModel* RobotModel::getJointModel(const std::string& name) JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - ROS_ERROR_NAMED(LOGNAME.c_str(), "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1117,7 +1125,7 @@ const LinkModel* RobotModel::getLinkModel(int index) const { if (index < 0 || index >= static_cast(link_model_vector_.size())) { - ROS_ERROR_NAMED(LOGNAME.c_str(), "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); return nullptr; } assert(link_model_vector_[index]->getLinkIndex() == index); @@ -1129,7 +1137,7 @@ LinkModel* RobotModel::getLinkModel(const std::string& name) LinkModelMap::const_iterator it = link_model_map_.find(name); if (it != link_model_map_.end()) return it->second; - ROS_ERROR_NAMED(LOGNAME.c_str(), "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(logger_robot_model, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1330,7 +1338,7 @@ void RobotModel::setKinematicsAllocators(const std::mapgetName() << " "; result.second[subs[i]] = allocators.find(subs[i]->getName())->second; } - ROS_DEBUG_NAMED(LOGNAME.c_str(), "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), + RCLCPP_DEBUG(logger_robot_model, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str()); } jmg->setSolverAllocators(result); From 80e49b00936e36a4640bb9303a02f775cfeacf92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 16 Mar 2019 20:23:24 +0100 Subject: [PATCH 09/12] robot_model use LOGGER Follow consensus about logging in moveit_core submodules --- .../include/moveit/robot_model/robot_model.h | 1 - .../robot_model/src/floating_joint_model.cpp | 5 +- .../robot_model/src/joint_model_group.cpp | 25 +++--- moveit_core/robot_model/src/robot_model.cpp | 77 ++++++++++--------- 4 files changed, 57 insertions(+), 51 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 10de8b48ef..83d0a52325 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -49,7 +49,6 @@ #include #include #include -#include #include #include diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 71e623279a..8893b475ee 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -39,11 +39,14 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" namespace moveit { namespace core { +rclcpp::Logger LOGGER = rclcpp::get_logger("robot_model"); + FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0) { type_ = FLOATING; @@ -170,7 +173,7 @@ bool FloatingJointModel::normalizeRotation(double* values) const double norm = sqrt(norm_sqr); if (norm < std::numeric_limits::epsilon() * 100.0) { - RCLCPP_WARN(logger_robot_model, "Quaternion is zero in RobotState representation. Setting to identity"); + RCLCPP_WARN(LOGGER, "Quaternion is zero in RobotState representation. Setting to identity"); values[3] = 0.0; values[4] = 0.0; values[5] = 0.0; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index 986447c6f8..e5b849231d 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -42,6 +42,7 @@ #include #include #include "order_robot_model_items.inc" +#include "rclcpp/rclcpp.hpp" namespace moveit { @@ -49,6 +50,8 @@ namespace core { namespace { +rclcpp::Logger LOGGER = rclcpp::get_logger("robot_model.jmg"); + // check if a parent or ancestor of joint is included in this group bool includesParent(const JointModel* joint, const JointModelGroup* group) { @@ -292,7 +295,7 @@ const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const LinkModelMapConst::const_iterator it = link_model_map_.find(name); if (it == link_model_map_.end()) { - RCLCPP_ERROR(logger_robot_model_jmg, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); + RCLCPP_ERROR(LOGGER, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -303,7 +306,7 @@ const JointModel* JointModelGroup::getJointModel(const std::string& name) const JointModelMapConst::const_iterator it = joint_model_map_.find(name); if (it == joint_model_map_.end()) { - RCLCPP_ERROR(logger_robot_model_jmg, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); + RCLCPP_ERROR(LOGGER, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str()); return nullptr; } return it->second; @@ -345,7 +348,7 @@ void JointModelGroup::getVariableRandomPositionsNearBy( if (iter != distance_map.end()) distance = iter->second; else { - RCLCPP_WARN(logger_robot_model_jmg, "Did not pass in distance for '%s'", + RCLCPP_WARN(LOGGER, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str()); } active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( @@ -503,7 +506,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const JointModelGroup* eef = parent_model_->getEndEffector(name); if (!eef) { - RCLCPP_ERROR(logger_robot_model_jmg, "Unable to find joint model group for eef"); + RCLCPP_ERROR(LOGGER, "Unable to find joint model group for eef"); return false; } const std::string& eef_parent = eef->getEndEffectorParentGroup().second; @@ -511,7 +514,7 @@ bool JointModelGroup::getEndEffectorTips(std::vector& tips) co const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent); if (!eef_link) { - RCLCPP_ERROR(logger_robot_model_jmg, "Unable to find end effector link for eef"); + RCLCPP_ERROR(LOGGER, "Unable to find end effector link for eef"); return false; } // insert eef_link into tips, maintaining a *sorted* vector, thus enabling use of std::lower_bound @@ -529,10 +532,10 @@ const LinkModel* JointModelGroup::getOnlyOneEndEffectorTip() const if (tips.size() == 1) return tips.front(); else if (tips.size() > 1) { - RCLCPP_ERROR(logger_robot_model_jmg, "More than one end effector tip found for joint model group, so cannot return only one"); + RCLCPP_ERROR(LOGGER, "More than one end effector tip found for joint model group, so cannot return only one"); } else { - RCLCPP_ERROR(logger_robot_model_jmg, "No end effector tips found in joint model group"); + RCLCPP_ERROR(LOGGER, "No end effector tips found in joint model group"); } return nullptr; } @@ -542,7 +545,7 @@ int JointModelGroup::getVariableGroupIndex(const std::string& variable) const VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable); if (it == joint_variables_index_map_.end()) { - RCLCPP_ERROR(logger_robot_model_jmg, "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); + RCLCPP_ERROR(LOGGER, "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str()); return -1; } return it->second; @@ -576,7 +579,7 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik // skip reported fixed joints if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED) continue; - RCLCPP_ERROR(logger_robot_model_jmg, "IK solver computes joint values for joint '%s' " + RCLCPP_ERROR(LOGGER, "IK solver computes joint values for joint '%s' " "but group '%s' does not contain such a joint.", ik_jnames[i].c_str(), getName().c_str()); return false; @@ -629,7 +632,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const if (tip_frames.empty()) { - RCLCPP_WARN(logger_robot_model_jmg, "Group %s has no tip frame(s)", name_.c_str()); + RCLCPP_WARN(LOGGER, "Group %s has no tip frame(s)", name_.c_str()); return false; } @@ -639,7 +642,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const // remove frame reference, if specified const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip; const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i]; - RCLCPP_WARN(logger_robot_model_jmg, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), + RCLCPP_WARN(LOGGER, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), tip_frame_local.c_str()); // Check if the IK solver's tip is the same as the frame of inquiry diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 347fc8c5c7..55aaa29f1d 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -45,12 +45,13 @@ #include #include #include "order_robot_model_items.inc" +#include "rclcpp/rclcpp.hpp" namespace moveit { namespace core { -const std::string LOGNAME = "robot_model"; +rclcpp::Logger LOGGER = rclcpp::get_logger("robot_model"); RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model) { @@ -90,40 +91,40 @@ void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf:: link_geometry_count_ = 0; variable_count_ = 0; model_name_ = urdf_model.getName(); - RCLCPP_INFO(logger_robot_model, "Loading robot model '%s'...", model_name_.c_str()); + RCLCPP_INFO(LOGGER, "Loading robot model '%s'...", model_name_.c_str()); if (urdf_model.getRoot()) { const urdf::Link* root_link_ptr = urdf_model.getRoot().get(); model_frame_ = root_link_ptr->name; - RCLCPP_DEBUG(logger_robot_model, "... building kinematic chain"); + RCLCPP_DEBUG(LOGGER, "... building kinematic chain"); root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model); if (root_joint_) root_link_ = root_joint_->getChildLinkModel(); - RCLCPP_DEBUG(logger_robot_model, "... building mimic joints"); + RCLCPP_DEBUG(LOGGER, "... building mimic joints"); buildMimic(urdf_model); - RCLCPP_DEBUG(logger_robot_model, "... computing joint indexing"); + RCLCPP_DEBUG(LOGGER, "... computing joint indexing"); buildJointInfo(); if (link_models_with_collision_geometry_vector_.empty()) { - RCLCPP_WARN(logger_robot_model, "No geometry is associated to any robot links"); + RCLCPP_WARN(LOGGER, "No geometry is associated to any robot links"); } // build groups - RCLCPP_DEBUG(logger_robot_model, "... constructing joint groups"); + RCLCPP_DEBUG(LOGGER, "... constructing joint groups"); buildGroups(srdf_model); - RCLCPP_DEBUG(logger_robot_model, "... constructing joint group states"); + RCLCPP_DEBUG(LOGGER, "... constructing joint group states"); buildGroupStates(srdf_model); // For debugging entire model // printModelInfo(std::cout); } else { - RCLCPP_WARN(logger_robot_model, "No root link found"); + RCLCPP_WARN(LOGGER, "No root link found"); } } @@ -342,13 +343,13 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) for (std::size_t j = 0; j < vn.size(); ++j) state[vn[j]] = jt->second[j]; else { - RCLCPP_ERROR(logger_robot_model, "The model for joint '%s' requires %d variable values, but only %d variable values were supplied in default state '%s' for group '%s'", + RCLCPP_ERROR(LOGGER, "The model for joint '%s' requires %d variable values, but only %d variable values were supplied in default state '%s' for group '%s'", jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), jmg->getName().c_str()); } } else { - RCLCPP_ERROR(logger_robot_model, "Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'", + RCLCPP_ERROR(LOGGER, "Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'", ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } } @@ -356,7 +357,7 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) jmg->addDefaultState(ds[i].name_, state); } else { - RCLCPP_ERROR(logger_robot_model, "Group state '%s' specified for group '%s', but that group does not exist", + RCLCPP_ERROR(LOGGER, "Group state '%s' specified for group '%s', but that group does not exist", ds[i].name_.c_str(), ds[i].group_.c_str()); } } @@ -377,12 +378,12 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount()) joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset); else { - RCLCPP_ERROR(logger_robot_model, "Join '%s' cannot mimic joint '%s' because they have different number of DOF", + RCLCPP_ERROR(LOGGER, "Join '%s' cannot mimic joint '%s' because they have different number of DOF", joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); } } else { - RCLCPP_ERROR(logger_robot_model, "Joint '%s' cannot mimic unknown joint '%s'", + RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic unknown joint '%s'", joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); } } @@ -407,7 +408,7 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) } if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic()) { - RCLCPP_ERROR(logger_robot_model, "Cycle found in joint that mimic each other. Ignoring all mimic joints."); + RCLCPP_ERROR(LOGGER, "Cycle found in joint that mimic each other. Ignoring all mimic joints."); for (std::size_t i = 0; i < joint_model_vector_.size(); ++i) joint_model_vector_[i]->setMimic(nullptr, 0.0, 0.0); change = false; @@ -437,7 +438,7 @@ const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - RCLCPP_ERROR(logger_robot_model, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -451,7 +452,7 @@ JointModelGroup* RobotModel::getEndEffector(const std::string& name) it = joint_model_group_map_.find(name); if (it != joint_model_group_map_.end() && it->second->isEndEffector()) return it->second; - RCLCPP_ERROR(logger_robot_model, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -467,7 +468,7 @@ const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) c JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - RCLCPP_ERROR(logger_robot_model, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -478,7 +479,7 @@ JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name); if (it == joint_model_group_map_.end()) { - RCLCPP_ERROR(logger_robot_model, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } return it->second; @@ -513,7 +514,7 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) added = true; processed[i] = true; if (!addJointModelGroup(group_configs[i])) { - RCLCPP_WARN(logger_robot_model, "Failed to add group '%s'", group_configs[i].name_.c_str()); + RCLCPP_WARN(LOGGER, "Failed to add group '%s'", group_configs[i].name_.c_str()); } } } @@ -521,7 +522,7 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) for (std::size_t i = 0; i < processed.size(); ++i) if (!processed[i]) { - RCLCPP_WARN(logger_robot_model, "Could not process group '%s' due to unmet subgroup dependencies", + RCLCPP_WARN(LOGGER, "Could not process group '%s' due to unmet subgroup dependencies", group_configs[i].name_.c_str()); } @@ -608,17 +609,17 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) if (jt->second != it->second) eef_parent_group = jt->second; else { - RCLCPP_ERROR(logger_robot_model, "Group '%s' for end-effector '%s' cannot be its own parent", + RCLCPP_ERROR(LOGGER, "Group '%s' for end-effector '%s' cannot be its own parent", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); } } else { - RCLCPP_ERROR(logger_robot_model, "Group '%s' was specified as parent group for end-effector '%s' but it does not include the parent link '%s'", + RCLCPP_ERROR(LOGGER, "Group '%s' was specified as parent group for end-effector '%s' but it does not include the parent link '%s'", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str()); } } else { - RCLCPP_ERROR(logger_robot_model, "Group name '%s' not found (specified as parent group for end-effector '%s')", + RCLCPP_ERROR(LOGGER, "Group name '%s' not found (specified as parent group for end-effector '%s')", eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); } } @@ -643,7 +644,7 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) } else { - RCLCPP_WARN(logger_robot_model, "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str()); + RCLCPP_WARN(LOGGER, "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str()); it->second->setEndEffectorParent("", eefs[k].parent_link_); } } @@ -655,7 +656,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) { if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end()) { - RCLCPP_WARN(logger_robot_model, "A group named '%s' already exists. Not adding.", gc.name_.c_str()); + RCLCPP_WARN(LOGGER, "A group named '%s' already exists. Not adding.", gc.name_.c_str()); return false; } @@ -752,7 +753,7 @@ bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc) if (jset.empty()) { - RCLCPP_WARN(logger_robot_model, "Group '%s' must have at least one valid joint", gc.name_.c_str()); + RCLCPP_WARN(LOGGER, "Group '%s' must have at least one valid joint", gc.name_.c_str()); return false; } @@ -897,7 +898,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const result = new FixedJointModel(urdf_joint->name); break; default: - RCLCPP_ERROR(logger_robot_model, "Unknown joint type: %d", (int)urdf_joint->type); + RCLCPP_ERROR(LOGGER, "Unknown joint type: %d", (int)urdf_joint->type); break; } } @@ -908,13 +909,13 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const { if (virtual_joints[i].child_link_ != child_link->name) { - RCLCPP_WARN(logger_robot_model, "Skipping virtual joint '%s' because its child frame '%s' does not match the URDF frame '%s'", + RCLCPP_WARN(LOGGER, "Skipping virtual joint '%s' because its child frame '%s' does not match the URDF frame '%s'", virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(), child_link->name.c_str()); } else if (virtual_joints[i].parent_frame_.empty()) { - RCLCPP_WARN(logger_robot_model, "Skipping virtual joint '%s' because its parent frame is empty", + RCLCPP_WARN(LOGGER, "Skipping virtual joint '%s' because its parent frame is empty", virtual_joints[i].name_.c_str()); } else @@ -938,7 +939,7 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const } if (!result) { - RCLCPP_INFO(logger_robot_model, "No root/virtual joint specified in SRDF. Assuming fixed joint"); + RCLCPP_INFO(LOGGER, "No root/virtual joint specified in SRDF. Assuming fixed joint"); result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT"); } } @@ -1070,7 +1071,7 @@ shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom) } break; default: - RCLCPP_ERROR(logger_robot_model, "Unknown geometry type: %d", (int)geom->type); + RCLCPP_ERROR(LOGGER, "Unknown geometry type: %d", (int)geom->type); break; } @@ -1092,7 +1093,7 @@ const JointModel* RobotModel::getJointModel(const std::string& name) const JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - RCLCPP_ERROR(logger_robot_model, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1100,7 +1101,7 @@ const JointModel* RobotModel::getJointModel(int index) const { if (index < 0 || index >= static_cast(joint_model_vector_.size())) { - RCLCPP_ERROR(logger_robot_model, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str()); return nullptr; } assert(joint_model_vector_[index]->getJointIndex() == index); @@ -1112,7 +1113,7 @@ JointModel* RobotModel::getJointModel(const std::string& name) JointModelMap::const_iterator it = joint_model_map_.find(name); if (it != joint_model_map_.end()) return it->second; - RCLCPP_ERROR(logger_robot_model, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1125,7 +1126,7 @@ const LinkModel* RobotModel::getLinkModel(int index) const { if (index < 0 || index >= static_cast(link_model_vector_.size())) { - RCLCPP_ERROR(logger_robot_model, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str()); return nullptr; } assert(link_model_vector_[index]->getLinkIndex() == index); @@ -1137,7 +1138,7 @@ LinkModel* RobotModel::getLinkModel(const std::string& name) LinkModelMap::const_iterator it = link_model_map_.find(name); if (it != link_model_map_.end()) return it->second; - RCLCPP_ERROR(logger_robot_model, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + RCLCPP_ERROR(LOGGER, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); return nullptr; } @@ -1338,7 +1339,7 @@ void RobotModel::setKinematicsAllocators(const std::mapgetName() << " "; result.second[subs[i]] = allocators.find(subs[i]->getName())->second; } - RCLCPP_DEBUG(logger_robot_model, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), + RCLCPP_DEBUG(LOGGER, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), ss.str().c_str()); } jmg->setSolverAllocators(result); From f6700ab1d6c314769a510381cd632e80235467ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 16 Mar 2019 20:28:26 +0100 Subject: [PATCH 10/12] Remove robot_model log.h --- .../include/moveit/robot_model/log.h | 54 ------------------- 1 file changed, 54 deletions(-) delete mode 100644 moveit_core/robot_model/include/moveit/robot_model/log.h diff --git a/moveit_core/robot_model/include/moveit/robot_model/log.h b/moveit_core/robot_model/include/moveit/robot_model/log.h deleted file mode 100644 index 4f03a9f734..0000000000 --- a/moveit_core/robot_model/include/moveit/robot_model/log.h +++ /dev/null @@ -1,54 +0,0 @@ -/********************************************************************* - * 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_MODEL_LOG_ -#define MOVEIT_CORE_ROBOT_MODEL_LOG_ - -#include "rclcpp/rclcpp.hpp" - -namespace moveit -{ -namespace core -{ - -// Logger -rclcpp::Logger logger_robot_model = rclcpp::get_logger("robot_model"); -rclcpp::Logger logger_robot_model_jmg = rclcpp::get_logger("robot_model.jmg"); - -}; -}; - -#endif From df31cab79ffb9e828ebd380fd04894765d58705d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 17 Mar 2019 00:50:31 +0100 Subject: [PATCH 11/12] robot_model, apply clang format --- .../include/moveit/robot_model/aabb.h | 68 +++++++------- .../moveit/robot_model/fixed_joint_model.h | 68 +++++++------- .../moveit/robot_model/floating_joint_model.h | 68 +++++++------- .../include/moveit/robot_model/joint_model.h | 4 +- .../moveit/robot_model/joint_model_group.h | 70 +++++++------- .../include/moveit/robot_model/link_model.h | 68 +++++++------- .../moveit/robot_model/planar_joint_model.h | 68 +++++++------- .../robot_model/prismatic_joint_model.h | 68 +++++++------- .../moveit/robot_model/revolute_joint_model.h | 68 +++++++------- .../include/moveit/robot_model/robot_model.h | 4 +- moveit_core/robot_model/src/aabb.cpp | 64 ++++++------- .../robot_model/src/fixed_joint_model.cpp | 64 ++++++------- .../robot_model/src/floating_joint_model.cpp | 66 ++++++------- moveit_core/robot_model/src/joint_model.cpp | 68 +++++++------- .../robot_model/src/joint_model_group.cpp | 93 ++++++++++--------- moveit_core/robot_model/src/link_model.cpp | 64 ++++++------- .../robot_model/src/planar_joint_model.cpp | 66 ++++++------- .../robot_model/src/prismatic_joint_model.cpp | 64 ++++++------- .../robot_model/src/revolute_joint_model.cpp | 66 ++++++------- moveit_core/robot_model/src/robot_model.cpp | 85 ++++++++++------- moveit_core/robot_model/test/test.cpp | 64 ++++++------- 21 files changed, 669 insertions(+), 649 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/aabb.h b/moveit_core/robot_model/include/moveit/robot_model/aabb.h index 2ed8ebf382..7024241395 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/aabb.h +++ b/moveit_core/robot_model/include/moveit/robot_model/aabb.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Martin Pecka */ @@ -50,7 +50,7 @@ class AABB : public Eigen::AlignedBox3d /** \brief Extend with a box transformed by the given transform. */ void extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box); }; -} -} +} // namespace core +} // namespace moveit #endif // MOVEIT_ROBOT_MODEL_AABB_H diff --git a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h index 265b13576c..4ecba760a9 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -66,7 +66,7 @@ class FixedJointModel : public JointModel void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const override; void computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const override; }; -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h index a39a8471e5..e46cc2fa97 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -89,7 +89,7 @@ class FloatingJointModel : public JointModel private: double angular_distance_weight_; }; -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index e2367077bd..93e0ec8458 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -521,7 +521,7 @@ class JointModel /** \brief Operator overload for printing variable bounds to a stream */ std::ostream& operator<<(std::ostream& out, const VariableBounds& b); -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index b4fbaeda76..5dd1dff470 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -735,7 +735,7 @@ class JointModelGroup /** \brief The names of the default states specified for this group in the SRDF */ std::vector default_states_names_; }; -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index decebc2a82..fb4ea46413 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -281,7 +281,7 @@ class LinkModel /** \brief Index of the transform for this link in the full robot frame */ int link_index_; }; -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h index 0900e0b8d9..f6cdb4d4a8 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -84,7 +84,7 @@ class PlanarJointModel : public JointModel private: double angular_distance_weight_; }; -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h index f745947e18..ca1993aba1 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -84,7 +84,7 @@ class PrismaticJointModel : public JointModel /** \brief The axis of the joint */ Eigen::Vector3d axis_; }; -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h index 33adc02839..e95ad72ca6 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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, Inc. 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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, Inc. 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: Ioan Sucan */ @@ -95,7 +95,7 @@ class RevoluteJointModel : public JointModel private: double x2_, y2_, z2_, xy_, xz_, yz_; }; -} -} +} // namespace core +} // namespace moveit #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 83d0a52325..81e42c2c14 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -612,8 +612,8 @@ class RobotModel /** \brief Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape object*/ shapes::ShapePtr constructShape(const urdf::Geometry* geom); }; -} -} +} // namespace core +} // namespace moveit namespace robot_model = moveit::core; namespace robot_state = moveit::core; diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index fa7955ed37..7d183512e9 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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: Martin Pecka */ diff --git a/moveit_core/robot_model/src/fixed_joint_model.cpp b/moveit_core/robot_model/src/fixed_joint_model.cpp index 2b514b503a..a5c8d288c3 100644 --- a/moveit_core/robot_model/src/fixed_joint_model.cpp +++ b/moveit_core/robot_model/src/fixed_joint_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 8893b475ee..fe13b6025e 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/joint_model.cpp b/moveit_core/robot_model/src/joint_model.cpp index 286b379b41..5fe5072381 100644 --- a/moveit_core/robot_model/src/joint_model.cpp +++ b/moveit_core/robot_model/src/joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ @@ -216,7 +216,7 @@ inline void printBoundHelper(std::ostream& out, double v) else out << v; } -} +} // namespace std::ostream& operator<<(std::ostream& out, const VariableBounds& b) { diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index e5b849231d..8bb9a59d55 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan, Dave Coleman */ @@ -97,7 +97,7 @@ bool jointPrecedes(const JointModel* a, const JointModel* b) return false; } -} +} // namespace JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Model::Group& config, const std::vector& unsorted_group_joints, @@ -347,9 +347,9 @@ void JointModelGroup::getVariableRandomPositionsNearBy( distance_map.find(active_joint_model_vector_[i]->getType()); if (iter != distance_map.end()) distance = iter->second; - else { - RCLCPP_WARN(LOGGER, "Did not pass in distance for '%s'", - active_joint_model_vector_[i]->getName().c_str()); + else + { + RCLCPP_WARN(LOGGER, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str()); } active_joint_model_vector_[i]->getVariableRandomPositionsNearBy( rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i], @@ -531,10 +531,12 @@ const LinkModel* JointModelGroup::getOnlyOneEndEffectorTip() const getEndEffectorTips(tips); if (tips.size() == 1) return tips.front(); - else if (tips.size() > 1) { + else if (tips.size() > 1) + { RCLCPP_ERROR(LOGGER, "More than one end effector tip found for joint model group, so cannot return only one"); } - else { + else + { RCLCPP_ERROR(LOGGER, "No end effector tips found in joint model group"); } return nullptr; @@ -559,7 +561,8 @@ void JointModelGroup::setDefaultIKTimeout(double ik_timeout) for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it) it->second.default_ik_timeout_ = ik_timeout; } -// Re-added this as its needed to compile robot_state, this has been removed on https://github.com/ros-planning/moveit/pull/1288/files +// Re-added this as its needed to compile robot_state, this has been removed on +// https://github.com/ros-planning/moveit/pull/1288/files void JointModelGroup::setDefaultIKAttempts(unsigned int ik_attempts) { group_kinematics_.first.default_ik_attempts_ = ik_attempts; @@ -579,9 +582,10 @@ bool JointModelGroup::computeIKIndexBijection(const std::vector& ik // skip reported fixed joints if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED) continue; - RCLCPP_ERROR(LOGGER, "IK solver computes joint values for joint '%s' " - "but group '%s' does not contain such a joint.", - ik_jnames[i].c_str(), getName().c_str()); + RCLCPP_ERROR(LOGGER, + "IK solver computes joint values for joint '%s' " + "but group '%s' does not contain such a joint.", + ik_jnames[i].c_str(), getName().c_str()); return false; } const JointModel* jm = getJointModel(ik_jnames[i]); @@ -642,8 +646,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const // remove frame reference, if specified const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip; const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i]; - RCLCPP_WARN(LOGGER, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), - tip_frame_local.c_str()); + RCLCPP_WARN(LOGGER, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), tip_frame_local.c_str()); // Check if the IK solver's tip is the same as the frame of inquiry if (tip_local != tip_frame_local) diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index a4435e425e..0fe5a94c77 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 9be6a69bfa..10a8ac5879 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp index fd5897905e..2f2166edb5 100644 --- a/moveit_core/robot_model/src/prismatic_joint_model.cpp +++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index a7afaa9b91..e435527568 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -1,37 +1,37 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Ioan A. Sucan -* Copyright (c) 2008-2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Ioan A. Sucan + * Copyright (c) 2008-2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 55aaa29f1d..c5e51196c3 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -108,7 +108,8 @@ void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf:: RCLCPP_DEBUG(LOGGER, "... computing joint indexing"); buildJointInfo(); - if (link_models_with_collision_geometry_vector_.empty()) { + if (link_models_with_collision_geometry_vector_.empty()) + { RCLCPP_WARN(LOGGER, "No geometry is associated to any robot links"); } @@ -123,7 +124,8 @@ void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf:: // For debugging entire model // printModelInfo(std::cout); } - else { + else + { RCLCPP_WARN(LOGGER, "No root link found"); } } @@ -194,7 +196,7 @@ void computeCommonRootsHelper(const JointModel* joint, std::vector& common_ computeCommonRootsHelper(ch[i], common_roots, size); } } -} +} // namespace void RobotModel::computeCommonRoots() { @@ -342,23 +344,29 @@ void RobotModel::buildGroupStates(const srdf::Model& srdf_model) if (vn.size() == jt->second.size()) for (std::size_t j = 0; j < vn.size(); ++j) state[vn[j]] = jt->second[j]; - else { - RCLCPP_ERROR(LOGGER, "The model for joint '%s' requires %d variable values, but only %d variable values were supplied in default state '%s' for group '%s'", - jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), - jmg->getName().c_str()); + else + { + RCLCPP_ERROR(LOGGER, + "The model for joint '%s' requires %d variable values, but only %d variable values were " + "supplied in default state '%s' for group '%s'", + jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), + jmg->getName().c_str()); } } - else { - RCLCPP_ERROR(LOGGER, "Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'", - ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); + else + { + RCLCPP_ERROR(LOGGER, + "Group state '%s' specifies value for joint '%s', but that joint is not part of group '%s'", + ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str()); } } if (!state.empty()) jmg->addDefaultState(ds[i].name_, state); } - else { + else + { RCLCPP_ERROR(LOGGER, "Group state '%s' specified for group '%s', but that group does not exist", - ds[i].name_.c_str(), ds[i].group_.c_str()); + ds[i].name_.c_str(), ds[i].group_.c_str()); } } } @@ -377,14 +385,16 @@ void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model) { if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount()) joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset); - else { + else + { RCLCPP_ERROR(LOGGER, "Join '%s' cannot mimic joint '%s' because they have different number of DOF", - joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); + joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); } } - else { - RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic unknown joint '%s'", - joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str()); + else + { + RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic unknown joint '%s'", joint_model_vector_[i]->getName().c_str(), + jm->mimic->joint_name.c_str()); } } } @@ -513,7 +523,8 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) { added = true; processed[i] = true; - if (!addJointModelGroup(group_configs[i])) { + if (!addJointModelGroup(group_configs[i])) + { RCLCPP_WARN(LOGGER, "Failed to add group '%s'", group_configs[i].name_.c_str()); } } @@ -521,9 +532,10 @@ void RobotModel::buildGroups(const srdf::Model& srdf_model) } for (std::size_t i = 0; i < processed.size(); ++i) - if (!processed[i]) { + if (!processed[i]) + { RCLCPP_WARN(LOGGER, "Could not process group '%s' due to unmet subgroup dependencies", - group_configs[i].name_.c_str()); + group_configs[i].name_.c_str()); } for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it) @@ -608,19 +620,24 @@ void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model) { if (jt->second != it->second) eef_parent_group = jt->second; - else { + else + { RCLCPP_ERROR(LOGGER, "Group '%s' for end-effector '%s' cannot be its own parent", - eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); + eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); } } - else { - RCLCPP_ERROR(LOGGER, "Group '%s' was specified as parent group for end-effector '%s' but it does not include the parent link '%s'", - eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str()); + else + { + RCLCPP_ERROR(LOGGER, + "Group '%s' was specified as parent group for end-effector '%s' but it does not include the " + "parent link '%s'", + eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str()); } } - else { + else + { RCLCPP_ERROR(LOGGER, "Group name '%s' not found (specified as parent group for end-effector '%s')", - eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); + eefs[k].parent_group_.c_str(), eefs[k].name_.c_str()); } } @@ -850,7 +867,7 @@ static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint) } return b; } -} +} // namespace JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link, const srdf::Model& srdf_model) @@ -909,14 +926,14 @@ JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const { if (virtual_joints[i].child_link_ != child_link->name) { - RCLCPP_WARN(LOGGER, "Skipping virtual joint '%s' because its child frame '%s' does not match the URDF frame '%s'", - virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(), - child_link->name.c_str()); + RCLCPP_WARN(LOGGER, + "Skipping virtual joint '%s' because its child frame '%s' does not match the URDF frame '%s'", + virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(), child_link->name.c_str()); } else if (virtual_joints[i].parent_frame_.empty()) { RCLCPP_WARN(LOGGER, "Skipping virtual joint '%s' because its parent frame is empty", - virtual_joints[i].name_.c_str()); + virtual_joints[i].name_.c_str()); } else { @@ -969,7 +986,7 @@ static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose) Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q); return af; } -} +} // namespace LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link) { @@ -1340,7 +1357,7 @@ void RobotModel::setKinematicsAllocators(const std::mapgetName())->second; } RCLCPP_DEBUG(LOGGER, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(), - ss.str().c_str()); + ss.str().c_str()); } jmg->setSolverAllocators(result); } diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index 70c59eca0c..56742a8784 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -1,36 +1,36 @@ /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2013, Willow Garage, Inc. -* 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 the 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. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Willow Garage, Inc. + * 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 the 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: Ioan Sucan */ From 77d2d9a398738c364e61cfb4ef53275bca41126a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 24 Mar 2019 01:24:40 +0100 Subject: [PATCH 12/12] moveit_core, robot_model: remove setDefaultIKAttempts --- moveit_core/robot_model/src/joint_model_group.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index a70f919fa8..7984ab0d28 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -561,14 +561,6 @@ void JointModelGroup::setDefaultIKTimeout(double ik_timeout) for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it) it->second.default_ik_timeout_ = ik_timeout; } -// Re-added this as its needed to compile robot_state, this has been removed on -// https://github.com/ros-planning/moveit/pull/1288/files -void JointModelGroup::setDefaultIKAttempts(unsigned int ik_attempts) -{ - group_kinematics_.first.default_ik_attempts_ = ik_attempts; - for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it) - it->second.default_ik_attempts_ = ik_attempts; -} bool JointModelGroup::computeIKIndexBijection(const std::vector& ik_jnames, std::vector& joint_bijection) const