diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 13174d3f1d..7a94008311 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -1,30 +1,55 @@ set(MOVEIT_LIB_NAME moveit_planning_scene) -add_library(${MOVEIT_LIB_NAME} src/planning_scene.cpp) +add_library(${MOVEIT_LIB_NAME} SHARED src/planning_scene.cpp) +#TODO: Fix the versioning set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC + ${octomap_msgs_INCLUDE_DIRS} + ${octomap_INCLUDE_DIRS} +) + target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_robot_state moveit_exceptions moveit_transforms moveit_collision_detection_fcl + moveit_collision_detection moveit_kinematic_constraints moveit_robot_trajectory moveit_trajectory_processing - ${LIBOCTOMAP_LIBRARIES} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) - -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + ${LIBOCTOMAP_LIBRARIES} + ${rclcpp_LIBRARIES} + ${rmw_implementation_LIBRARIES} + ${urdfdom_LIBRARIES} + ${urdfdom_headers_LIBRARIES} + ${Boost_LIBRARIES} + ${octomap_msgs_LIBRARIES} + ${OCTOMAP_LIBRARIES} +) 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) -if(CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) find_package(moveit_resources REQUIRED) + find_package(ament_cmake_gtest REQUIRED) include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp) - target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME}) + if(UNIX OR APPLE) + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_BINARY_DIR}/../utils:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection") + endif() + + ament_add_gtest(test_planning_scene test/test_planning_scene.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + target_link_libraries(test_planning_scene + ${MOVEIT_LIB_NAME} + ${srdfdom_LIBRARIES} + ${urdf_LIBRARIES} + ${geometric_shapes_LIBRARIES} + ${OCTOMAP_LIBRARIES} + ) endif() diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index d52b7f0b12..b635ef73fd 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -51,11 +51,12 @@ #include #include #include -#include +#include #include #include #include #include +#include "rclcpp/rclcpp.hpp" /** \brief This namespace includes the central class for representing planning contexts */ namespace planning_scene @@ -76,10 +77,10 @@ typedef boost::function StateFeasibi typedef boost::function MotionFeasibilityFn; /** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */ -typedef std::map ObjectColorMap; +typedef std::map ObjectColorMap; /** \brief A map from object names (e.g., attached bodies, collision objects) to their types */ -typedef std::map ObjectTypeMap; +typedef std::map ObjectTypeMap; /** \brief This class maintains the representation of the environment as seen by a planning instance. The environment @@ -705,7 +706,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Construct a message (\e scene) with the data requested in \e comp. If all options in \e comp are filled, this will be a complete planning scene message */ - void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene, const moveit_msgs::msg::PlanningSceneComponents& comp) const; + void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene, + const moveit_msgs::msg::PlanningSceneComponents& comp) const; /** \brief Construct a message (\e collision_object) with the collision object data from the planning_scene for the * requested object*/ @@ -722,10 +724,11 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from /** \brief Construct a vector of messages (\e attached_collision_objects) with the attached collision object data for * all objects in planning_scene */ - void getAttachedCollisionObjectMsgs(std::vector& attached_collision_objs) const; + void + getAttachedCollisionObjectMsgs(std::vector& attached_collision_objs) const; /** \brief Construct a message (\e octomap) with the octomap data from the planning_scene */ - bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const; + bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) const; /** \brief Construct a vector of messages (\e object_colors) with the colors of the objects from the planning_scene */ void getObjectColorMsgs(std::vector& object_colors) const; @@ -750,8 +753,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld& world); - void processOctomapMsg(const octomap_msgs::OctomapWithPose& map); - void processOctomapMsg(const octomap_msgs::Octomap& map); + void processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& map); + void processOctomapMsg(const octomap_msgs::msg::Octomap& map); void processOctomapPtr(const std::shared_ptr& octree, const Eigen::Isometry3d& t); /** @@ -775,15 +778,15 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from bool hasObjectColor(const std::string& id) const; - const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const; - void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color); + const std_msgs::msg::ColorRGBA& getObjectColor(const std::string& id) const; + void setObjectColor(const std::string& id, const std_msgs::msg::ColorRGBA& color); void removeObjectColor(const std::string& id); void getKnownObjectColors(ObjectColorMap& kc) const; bool hasObjectType(const std::string& id) const; - const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const; - void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type); + const object_recognition_msgs::msg::ObjectType& getObjectType(const std::string& id) const; + void setObjectType(const std::string& id, const object_recognition_msgs::msg::ObjectType& type); void removeObjectType(const std::string& id); void getKnownObjectTypes(ObjectTypeMap& kc) const; @@ -855,7 +858,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ - bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "", bool verbose = false) const; + bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "", + bool verbose = false) const; /** \brief Check if a given state is valid. This means checking for collisions and feasibility */ bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const; @@ -891,9 +895,9 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the * passed in trajectory. */ bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints, - const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + const moveit_msgs::msg::Constraints& path_constraints, + const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = NULL) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the @@ -915,9 +919,9 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from * constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the * passed in trajectory. */ bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory, - const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints, - const std::string& group = "", bool verbose = false, - std::vector* invalid_index = NULL) const; + const moveit_msgs::msg::Constraints& path_constraints, + const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "", + bool verbose = false, std::vector* invalid_index = NULL) const; /** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and * constraint satisfaction). */ diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 1efa05368a..2dff70070c 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -50,6 +50,8 @@ namespace planning_scene { +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("planning_scene"); + const std::string PlanningScene::OCTOMAP_NS = ""; const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)"; @@ -123,6 +125,7 @@ bool PlanningScene::isEmpty(const moveit_msgs::msg::PlanningSceneWorld& msg) PlanningScene::PlanningScene(const robot_model::RobotModelConstPtr& robot_model, const collision_detection::WorldPtr& world) + : robot_model_(robot_model), world_(world), world_const_(world) { initialize(); @@ -352,9 +355,9 @@ bool PlanningScene::setActiveCollisionDetector(const std::string& collision_dete } else { - ROS_ERROR_NAMED(LOGNAME, "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. " - "Keeping existing active collision detector '%s'", - collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. " + "Keeping existing active collision detector '%s'", + collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); return false; } } @@ -373,8 +376,8 @@ PlanningScene::getCollisionWorld(const std::string& collision_detector_name) con CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionWorld named '%s'. Returning active CollisionWorld '%s' instead", - collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Could not get CollisionWorld named '%s'. Returning active CollisionWorld '%s' instead", + collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); return active_collision_->cworld_const_; } @@ -387,8 +390,8 @@ PlanningScene::getCollisionRobot(const std::string& collision_detector_name) con CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead", - collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead", + collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); return active_collision_->getCollisionRobot(); } @@ -401,9 +404,9 @@ PlanningScene::getCollisionRobotUnpadded(const std::string& collision_detector_n CollisionDetectorConstIterator it = collision_.find(collision_detector_name); if (it == collision_.end()) { - ROS_ERROR_NAMED(LOGNAME, "Could not get CollisionRobotUnpadded named '%s'. " - "Returning active CollisionRobotUnpadded '%s' instead", - collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); + RCLCPP_ERROR(LOGGER, "Could not get CollisionRobotUnpadded named '%s'. " + "Returning active CollisionRobotUnpadded '%s' instead", + collision_detector_name.c_str(), active_collision_->alloc_->getName().c_str()); return active_collision_->getCollisionRobotUnpadded(); } @@ -749,7 +752,7 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce } scene_msg.world.collision_objects.clear(); - scene_msg.world.octomap = octomap_msgs::OctomapWithPose(); + scene_msg.world.octomap = octomap_msgs::msg::OctomapWithPose(); if (world_diff_) { @@ -786,24 +789,24 @@ class ShapeVisitorAddToCollisionObject : public boost::static_visitor { } - void setPoseMessage(const geometry_msgs::Pose* pose) + void setPoseMessage(const geometry_msgs::msg::Pose* pose) { pose_ = pose; } - void operator()(const shape_msgs::Plane& shape_msg) const + void operator()(const shape_msgs::msg::Plane& shape_msg) const { obj_->planes.push_back(shape_msg); obj_->plane_poses.push_back(*pose_); } - void operator()(const shape_msgs::Mesh& shape_msg) const + void operator()(const shape_msgs::msg::Mesh& shape_msg) const { obj_->meshes.push_back(shape_msg); obj_->mesh_poses.push_back(*pose_); } - void operator()(const shape_msgs::SolidPrimitive& shape_msg) const + void operator()(const shape_msgs::msg::SolidPrimitive& shape_msg) const { obj_->primitives.push_back(shape_msg); obj_->primitive_poses.push_back(*pose_); @@ -811,7 +814,7 @@ class ShapeVisitorAddToCollisionObject : public boost::static_visitor private: moveit_msgs::msg::CollisionObject* obj_; - const geometry_msgs::Pose* pose_; + const geometry_msgs::msg::Pose* pose_; }; } // namespace @@ -829,7 +832,7 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::msg::CollisionObject& col shapes::ShapeMsg sm; if (constructMsgFromShape(obj->shapes_[j].get(), sm)) { - geometry_msgs::Pose p = tf2::toMsg(obj->shape_poses_[j]); + geometry_msgs::msg::Pose p = tf2::toMsg(obj->shape_poses_[j]); sv.setPoseMessage(&p); boost::apply_visitor(sv, sm); } @@ -879,10 +882,10 @@ void PlanningScene::getAttachedCollisionObjectMsgs( attachedBodiesToAttachedCollisionObjectMsgs(attached_bodies, attached_collision_objs); } -bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const +bool PlanningScene::getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) const { octomap.header.frame_id = getPlanningFrame(); - octomap.octomap = octomap_msgs::Octomap(); + octomap.octomap = octomap_msgs::msg::Octomap(); collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); if (map) @@ -894,8 +897,8 @@ bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const octomap.origin = tf2::toMsg(map->shape_poses_[0]); return true; } - ROS_ERROR_NAMED(LOGNAME, "Unexpected number of shapes in octomap collision object. Not including '%s' object", - OCTOMAP_NS.c_str()); + RCLCPP_ERROR(LOGGER, "Unexpected number of shapes in octomap collision object. Not including '%s' object", + OCTOMAP_NS.c_str()); } return false; } @@ -1024,7 +1027,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl; if (hasObjectColor(ns[i])) { - const std_msgs::ColorRGBA& c = getObjectColor(ns[i]); + const std_msgs::msg::ColorRGBA& c = getObjectColor(ns[i]); out << c.r << " " << c.g << " " << c.b << " " << c.a << std::endl; } else @@ -1044,7 +1047,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet { if (!in.good() || in.eof()) { - ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading scene geometry"); + RCLCPP_ERROR(LOGGER, "Bad input stream when loading scene geometry"); return false; } std::getline(in, name_); @@ -1054,7 +1057,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet in >> marker; if (!in.good() || in.eof()) { - ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading marker in scene geometry"); + RCLCPP_ERROR(LOGGER, "Bad input stream when loading marker in scene geometry"); return false; } if (marker == "*") @@ -1063,7 +1066,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet std::getline(in, ns); if (!in.good() || in.eof()) { - ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading ns in scene geometry"); + RCLCPP_ERROR(LOGGER, "Bad input stream when loading ns in scene geometry"); return false; } boost::algorithm::trim(ns); @@ -1074,24 +1077,24 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet shapes::Shape* s = shapes::constructShapeFromText(in); if (!s) { - ROS_ERROR_NAMED(LOGNAME, "Failed to load shape from scene file"); + RCLCPP_ERROR(LOGGER, "Failed to load shape from scene file"); return false; } double x, y, z, rx, ry, rz, rw; if (!(in >> x >> y >> z)) { - ROS_ERROR_NAMED(LOGNAME, "Improperly formatted translation in scene geometry file"); + RCLCPP_ERROR(LOGGER, "Improperly formatted translation in scene geometry file"); return false; } if (!(in >> rx >> ry >> rz >> rw)) { - ROS_ERROR_NAMED(LOGNAME, "Improperly formatted rotation in scene geometry file"); + RCLCPP_ERROR(LOGGER, "Improperly formatted rotation in scene geometry file"); return false; } float r, g, b, a; if (!(in >> r >> g >> b >> a)) { - ROS_ERROR_NAMED(LOGNAME, "Improperly formatted color in scene geometry file"); + RCLCPP_ERROR(LOGGER, "Improperly formatted color in scene geometry file"); return false; } if (s) @@ -1102,7 +1105,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet world_->addToObject(ns, shapes::ShapePtr(s), pose); if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f) { - std_msgs::ColorRGBA color; + std_msgs::msg::ColorRGBA color; color.r = r; color.g = g; color.b = b; @@ -1119,7 +1122,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } else { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unknown marker in scene geometry file: " << marker); + RCLCPP_ERROR(LOGGER, "Unknown marker in scene geometry file: %s ", marker.c_str()); return false; } } while (true); @@ -1146,11 +1149,12 @@ void PlanningScene::setCurrentState(const moveit_msgs::msg::RobotState& state) for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i) { - if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::msg::CollisionObject::ADD) + if (!state.is_diff && + state.attached_collision_objects[i].object.operation != moveit_msgs::msg::CollisionObject::ADD) { - ROS_ERROR_NAMED(LOGNAME, "The specified RobotState is not marked as is_diff. " - "The request to modify the object '%s' is not supported. Object is ignored.", - state.attached_collision_objects[i].object.id.c_str()); + RCLCPP_ERROR(LOGGER, "The specified RobotState is not marked as is_diff. " + "The request to modify the object '%s' is not supported. Object is ignored.", + state.attached_collision_objects[i].object.id.c_str()); continue; } processAttachedCollisionObjectMsg(state.attached_collision_objects[i]); @@ -1237,13 +1241,13 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen { bool result = true; - ROS_DEBUG_NAMED(LOGNAME, "Adding planning scene diff"); + RCLCPP_DEBUG(LOGGER, "Adding planning scene diff"); if (!scene_msg.name.empty()) name_ = scene_msg.name; if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName()) - ROS_WARN_NAMED(LOGNAME, "Setting the scene for model '%s' but model '%s' is loaded.", - scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str()); + RCLCPP_WARN(LOGGER, "Setting the scene for model '%s' but model '%s' is loaded.", + scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str()); // there is at least one transform in the list of fixed transform: from model frame to itself; // if the list is empty, then nothing has been set @@ -1294,12 +1298,12 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& scene_msg) { - ROS_DEBUG_NAMED(LOGNAME, "Setting new planning scene: '%s'", scene_msg.name.c_str()); + RCLCPP_DEBUG(LOGGER, "Setting new planning scene: '%s'", scene_msg.name.c_str()); name_ = scene_msg.name; if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name != getRobotModel()->getName()) - ROS_WARN_NAMED(LOGNAME, "Setting the scene for model '%s' but model '%s' is loaded.", - scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str()); + RCLCPP_WARN(LOGGER, "Setting the scene for model '%s' but model '%s' is loaded.", + scene_msg.robot_model_name.c_str(), getRobotModel()->getName().c_str()); if (parent_) decoupleParent(); @@ -1342,7 +1346,7 @@ bool PlanningScene::usePlanningSceneMsg(const moveit_msgs::msg::PlanningScene& s return setPlanningSceneMsg(scene_msg); } -void PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map) +void PlanningScene::processOctomapMsg(const octomap_msgs::msg::Octomap& map) { // each octomap replaces any previous one world_->removeObject(OCTOMAP_NS); @@ -1352,7 +1356,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::Octomap& map) if (map.id != "OcTree") { - ROS_ERROR_NAMED(LOGNAME, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str()); + RCLCPP_ERROR(LOGGER, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str()); return; } @@ -1380,7 +1384,7 @@ void PlanningScene::removeAllCollisionObjects() } } -void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map) +void PlanningScene::processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& map) { // each octomap replaces any previous one world_->removeObject(OCTOMAP_NS); @@ -1390,7 +1394,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map) if (map.octomap.id != "OcTree") { - ROS_ERROR_NAMED(LOGNAME, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str()); + RCLCPP_ERROR(LOGGER, "Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str()); return; } @@ -1437,15 +1441,16 @@ void PlanningScene::processOctomapPtr(const std::shared_ptrhasLinkModel(object.link_name)) + if (object.object.operation == moveit_msgs::msg::CollisionObject::ADD && + !getRobotModel()->hasLinkModel(object.link_name)) { - ROS_ERROR_NAMED(LOGNAME, "Unable to attach a body to link '%s' (link not found)", object.link_name.c_str()); + RCLCPP_ERROR(LOGGER, "Unable to attach a body to link '%s' (link not found)", object.link_name.c_str()); return false; } if (object.object.id == OCTOMAP_NS) { - ROS_ERROR_NAMED(LOGNAME, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); + RCLCPP_ERROR(LOGGER, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); return false; } @@ -1461,22 +1466,22 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At { if (object.object.primitives.size() != object.object.primitive_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of primitive shapes does not match number of poses " - "in attached collision object message"); + RCLCPP_ERROR(LOGGER, "Number of primitive shapes does not match number of poses " + "in attached collision object message"); return false; } if (object.object.meshes.size() != object.object.mesh_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of meshes does not match number of poses " - "in attached collision object message"); + RCLCPP_ERROR(LOGGER, "Number of meshes does not match number of poses " + "in attached collision object message"); return false; } if (object.object.planes.size() != object.object.plane_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of planes does not match number of poses " - "in attached collision object message"); + RCLCPP_ERROR(LOGGER, "Number of planes does not match number of poses " + "in attached collision object message"); return false; } @@ -1493,8 +1498,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At collision_detection::CollisionWorld::ObjectConstPtr obj = world_->getObject(object.object.id); if (obj) { - ROS_DEBUG_NAMED(LOGNAME, "Attaching world object '%s' to link '%s'", object.object.id.c_str(), - object.link_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Attaching world object '%s' to link '%s'", object.object.id.c_str(), + object.link_name.c_str()); // extract the shapes from the world shapes = obj->shapes_; @@ -1509,9 +1514,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At } else { - ROS_ERROR_NAMED(LOGNAME, "Attempting to attach object '%s' to link '%s' but no geometry specified " - "and such an object does not exist in the collision world", - object.object.id.c_str(), object.link_name.c_str()); + RCLCPP_ERROR(LOGGER, "Attempting to attach object '%s' to link '%s' but no geometry specified " + "and such an object does not exist in the collision world", + object.object.id.c_str(), object.link_name.c_str()); return false; } } @@ -1521,13 +1526,17 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At if (world_->removeObject(object.object.id)) { if (object.object.operation == moveit_msgs::msg::CollisionObject::ADD) - ROS_DEBUG_NAMED(LOGNAME, "Removing world object with the same name as newly attached object: '%s'", - object.object.id.c_str()); + { + RCLCPP_DEBUG(LOGGER, "Removing world object with the same name as newly attached object: '%s'", + object.object.id.c_str()); + } else - ROS_WARN_NAMED(LOGNAME, - "You tried to append geometry to an attached object that is actually a world object ('%s'). " - "World geometry is ignored.", - object.object.id.c_str()); + { + RCLCPP_WARN(LOGGER, + "You tried to append geometry to an attached object that is actually a world object ('%s'). " + "World geometry is ignored.", + object.object.id.c_str()); + } } for (std::size_t i = 0; i < object.object.primitives.size(); ++i) @@ -1576,8 +1585,8 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At if (shapes.empty()) { - ROS_ERROR_NAMED(LOGNAME, "There is no geometry to attach to link '%s' as part of attached body '%s'", - object.link_name.c_str(), object.object.id.c_str()); + RCLCPP_ERROR(LOGGER, "There is no geometry to attach to link '%s' as part of attached body '%s'", + object.link_name.c_str(), object.object.id.c_str()); return false; } @@ -1589,20 +1598,19 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At { // there should not exist an attached object with this name if (robot_state_->clearAttachedBody(object.object.id)) - ROS_DEBUG_NAMED(LOGNAME, "The robot state already had an object named '%s' attached to link '%s'. " - "The object was replaced.", - object.object.id.c_str(), object.link_name.c_str()); + RCLCPP_DEBUG(LOGGER, "The robot state already had an object named '%s' attached to link '%s'. " + "The object was replaced.", + object.object.id.c_str(), object.link_name.c_str()); robot_state_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, object.detach_posture); - ROS_DEBUG_NAMED(LOGNAME, "Attached object '%s' to link '%s'", object.object.id.c_str(), - object.link_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Attached object '%s' to link '%s'", object.object.id.c_str(), object.link_name.c_str()); } else { const robot_state::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id); shapes.insert(shapes.end(), ab->getShapes().begin(), ab->getShapes().end()); poses.insert(poses.end(), ab->getFixedTransforms().begin(), ab->getFixedTransforms().end()); - trajectory_msgs::JointTrajectory detach_posture = + trajectory_msgs::msg::JointTrajectory detach_posture = object.detach_posture.joint_names.empty() ? ab->getDetachPosture() : object.detach_posture; const std::set& ab_touch_links = ab->getTouchLinks(); robot_state_->clearAttachedBody(object.object.id); @@ -1611,14 +1619,16 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At else robot_state_->attachBody(object.object.id, shapes, poses, object.touch_links, object.link_name, detach_posture); - ROS_DEBUG_NAMED(LOGNAME, "Added shapes to object '%s' attached to link '%s'", object.object.id.c_str(), - object.link_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Added shapes to object '%s' attached to link '%s'", object.object.id.c_str(), + object.link_name.c_str()); } return true; } else - ROS_ERROR_NAMED(LOGNAME, "Robot state is not compatible with robot model. This could be fatal."); + { + RCLCPP_ERROR(LOGGER, "Robot state is not compatible with robot model. This could be fatal."); + } } else if (object.object.operation == moveit_msgs::msg::CollisionObject::REMOVE) { @@ -1662,15 +1672,17 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At robot_state_->clearAttachedBody(name); if (world_->hasObject(name)) - ROS_WARN_NAMED(LOGNAME, - "The collision world already has an object with the same name as the body about to be detached. " - "NOT adding the detached body '%s' to the collision world.", - object.object.id.c_str()); + { + RCLCPP_WARN(LOGGER, + "The collision world already has an object with the same name as the body about to be detached. " + "NOT adding the detached body '%s' to the collision world.", + object.object.id.c_str()); + } else { world_->addToObject(name, shapes, poses); - ROS_DEBUG_NAMED(LOGNAME, "Detached object '%s' from link '%s' and added it back in the collision world", - name.c_str(), object.link_name.c_str()); + RCLCPP_DEBUG(LOGGER, "Detached object '%s' from link '%s' and added it back in the collision world", + name.c_str(), object.link_name.c_str()); } } if (!attached_bodies.empty() || object.object.id.empty()) @@ -1678,11 +1690,11 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At } else if (object.object.operation == moveit_msgs::msg::CollisionObject::MOVE) { - ROS_ERROR_NAMED(LOGNAME, "Move for attached objects not yet implemented"); + RCLCPP_ERROR(LOGGER, "Move for attached objects not yet implemented"); } else { - ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", object.object.operation); + RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", object.object.operation); } return false; @@ -1692,11 +1704,12 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::msg::CollisionO { if (object.id == OCTOMAP_NS) { - ROS_ERROR_NAMED(LOGNAME, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); + RCLCPP_ERROR(LOGGER, "The ID '%s' cannot be used for collision objects (name reserved)", OCTOMAP_NS.c_str()); return false; } - if (object.operation == moveit_msgs::msg::CollisionObject::ADD || object.operation == moveit_msgs::msg::CollisionObject::APPEND) + if (object.operation == moveit_msgs::msg::CollisionObject::ADD || + object.operation == moveit_msgs::msg::CollisionObject::APPEND) { return processCollisionObjectAdd(object); } @@ -1709,7 +1722,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::msg::CollisionO return processCollisionObjectMove(object); } - ROS_ERROR_NAMED(LOGNAME, "Unknown collision object operation: %d", object.operation); + RCLCPP_ERROR(LOGGER, "Unknown collision object operation: %d", object.operation); return false; } @@ -1717,26 +1730,26 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::msg::CollisionO { if (object.primitives.empty() && object.meshes.empty() && object.planes.empty()) { - ROS_ERROR_NAMED(LOGNAME, "There are no shapes specified in the collision object message"); + RCLCPP_ERROR(LOGGER, "There are no shapes specified in the collision object message"); return false; } if (object.primitives.size() != object.primitive_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of primitive shapes does not match number of poses " - "in collision object message"); + RCLCPP_ERROR(LOGGER, "Number of primitive shapes does not match number of poses " + "in collision object message"); return false; } if (object.meshes.size() != object.mesh_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of meshes does not match number of poses in collision object message"); + RCLCPP_ERROR(LOGGER, "Number of meshes does not match number of poses in collision object message"); return false; } if (object.planes.size() != object.plane_poses.size()) { - ROS_ERROR_NAMED(LOGNAME, "Number of planes does not match number of poses in collision object message"); + RCLCPP_ERROR(LOGGER, "Number of planes does not match number of poses in collision object message"); return false; } @@ -1801,8 +1814,8 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision if (world_->hasObject(object.id)) { if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty()) - ROS_WARN_NAMED(LOGNAME, "Move operation for object '%s' ignores the geometry specified in the message.", - object.id.c_str()); + RCLCPP_WARN(LOGGER, "Move operation for object '%s' ignores the geometry specified in the message.", + object.id.c_str()); const Eigen::Isometry3d& t = getTransforms().getTransform(object.header.frame_id); EigenSTL::vector_Isometry3d new_poses; @@ -1835,15 +1848,15 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision } else { - ROS_ERROR_NAMED(LOGNAME, "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). " - "Not moving.", - new_poses.size(), object.id.c_str(), obj->shapes_.size()); + RCLCPP_ERROR(LOGGER, "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). " + "Not moving.", + new_poses.size(), object.id.c_str(), obj->shapes_.size()); return false; } return true; } - ROS_ERROR_NAMED(LOGNAME, "World object '%s' does not exist. Cannot move.", object.id.c_str()); + RCLCPP_ERROR(LOGGER, "World object '%s' does not exist. Cannot move.", object.id.c_str()); return false; } @@ -1874,7 +1887,7 @@ const Eigen::Isometry3d& PlanningScene::getFrameTransform(const robot_state::Rob collision_detection::World::ObjectConstPtr obj = getWorld()->getObject(id); if (obj->shape_poses_.size() > 1) { - ROS_WARN_NAMED(LOGNAME, "More than one shapes in object '%s'. Using first one to decide transform", id.c_str()); + RCLCPP_WARN(LOGGER, "More than one shapes in object '%s'. Using first one to decide transform", id.c_str()); return obj->shape_poses_[0]; } else if (obj->shape_poses_.size() == 1) @@ -1913,7 +1926,7 @@ bool PlanningScene::hasObjectType(const std::string& id) const return false; } -const object_recognition_msgs::ObjectType& PlanningScene::getObjectType(const std::string& id) const +const object_recognition_msgs::msg::ObjectType& PlanningScene::getObjectType(const std::string& id) const { if (object_types_) { @@ -1923,11 +1936,11 @@ const object_recognition_msgs::ObjectType& PlanningScene::getObjectType(const st } if (parent_) return parent_->getObjectType(id); - static const object_recognition_msgs::ObjectType EMPTY; + static const object_recognition_msgs::msg::ObjectType EMPTY; return EMPTY; } -void PlanningScene::setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type) +void PlanningScene::setObjectType(const std::string& id, const object_recognition_msgs::msg::ObjectType& type) { if (!object_types_) object_types_.reset(new ObjectTypeMap()); @@ -1960,7 +1973,7 @@ bool PlanningScene::hasObjectColor(const std::string& id) const return false; } -const std_msgs::ColorRGBA& PlanningScene::getObjectColor(const std::string& id) const +const std_msgs::msg::ColorRGBA& PlanningScene::getObjectColor(const std::string& id) const { if (object_colors_) { @@ -1970,7 +1983,7 @@ const std_msgs::ColorRGBA& PlanningScene::getObjectColor(const std::string& id) } if (parent_) return parent_->getObjectColor(id); - static const std_msgs::ColorRGBA EMPTY; + static const std_msgs::msg::ColorRGBA EMPTY; return EMPTY; } @@ -1984,11 +1997,11 @@ void PlanningScene::getKnownObjectColors(ObjectColorMap& kc) const kc[it->first] = it->second; } -void PlanningScene::setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color) +void PlanningScene::setObjectColor(const std::string& id, const std_msgs::msg::ColorRGBA& color) { if (id.empty()) { - ROS_ERROR_NAMED(LOGNAME, "Cannot set color of object with empty id."); + RCLCPP_ERROR(LOGGER, "Cannot set color of object with empty id."); return; } if (!object_colors_) @@ -2002,7 +2015,8 @@ void PlanningScene::removeObjectColor(const std::string& id) object_colors_->erase(id); } -bool PlanningScene::isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group, bool verbose) const +bool PlanningScene::isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group, + bool verbose) const { robot_state::RobotState s(getCurrentState()); robot_state::robotStateMsgToRobotState(getTransforms(), state, s); @@ -2045,16 +2059,16 @@ bool PlanningScene::isStateFeasible(const robot_state::RobotState& state, bool v return true; } -bool PlanningScene::isStateConstrained(const moveit_msgs::msg::RobotState& state, const moveit_msgs::msg::Constraints& constr, - bool verbose) const +bool PlanningScene::isStateConstrained(const moveit_msgs::msg::RobotState& state, + const moveit_msgs::msg::Constraints& constr, bool verbose) const { robot_state::RobotState s(getCurrentState()); robot_state::robotStateMsgToRobotState(getTransforms(), state, s); return isStateConstrained(s, constr, verbose); } -bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, const moveit_msgs::msg::Constraints& constr, - bool verbose) const +bool PlanningScene::isStateConstrained(const robot_state::RobotState& state, + const moveit_msgs::msg::Constraints& constr, bool verbose) const { kinematic_constraints::KinematicConstraintSetPtr ks( new kinematic_constraints::KinematicConstraintSet(getRobotModel())); @@ -2085,7 +2099,8 @@ bool PlanningScene::isStateValid(const robot_state::RobotState& state, const std return isStateValid(state, EMP_CONSTRAINTS, group, verbose); } -bool PlanningScene::isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group, bool verbose) const +bool PlanningScene::isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group, + bool verbose) const { static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS; return isStateValid(state, EMP_CONSTRAINTS, group, verbose); @@ -2121,8 +2136,8 @@ bool PlanningScene::isStateValid(const robot_state::RobotState& state, } bool PlanningScene::isPathValid(const moveit_msgs::msg::RobotState& start_state, - const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& group, bool verbose, - std::vector* invalid_index) const + const moveit_msgs::msg::RobotTrajectory& trajectory, const std::string& group, + bool verbose, std::vector* invalid_index) const { static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS; static const std::vector EMP_CONSTRAINTS_VECTOR; @@ -2151,8 +2166,8 @@ bool PlanningScene::isPathValid(const moveit_msgs::msg::RobotState& start_state, bool PlanningScene::isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, - const std::vector& goal_constraints, const std::string& group, - bool verbose, std::vector* invalid_index) const + const std::vector& goal_constraints, + const std::string& group, bool verbose, std::vector* invalid_index) const { robot_trajectory::RobotTrajectory t(getRobotModel(), group); robot_state::RobotState start(getCurrentState()); @@ -2163,8 +2178,8 @@ bool PlanningScene::isPathValid(const moveit_msgs::msg::RobotState& start_state, bool PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory& trajectory, const moveit_msgs::msg::Constraints& path_constraints, - const std::vector& goal_constraints, const std::string& group, - bool verbose, std::vector* invalid_index) const + const std::vector& goal_constraints, + const std::string& group, bool verbose, std::vector* invalid_index) const { bool result = true; if (invalid_index) @@ -2208,7 +2223,7 @@ bool PlanningScene::isPathValid(const robot_trajectory::RobotTrajectory& traject if (!found) { if (verbose) - ROS_INFO_NAMED(LOGNAME, "Goal not satisfied"); + RCLCPP_INFO(LOGGER, "Goal not satisfied"); if (invalid_index) invalid_index->push_back(i); result = false;