diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 37ab0f515b..73425a27dd 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -1,4 +1,7 @@ -add_library(moveit_planning_scene SHARED src/planning_scene.cpp) +add_library(moveit_planning_scene SHARED + src/planning_scene.cpp + src/utilities.cpp +) target_include_directories(moveit_planning_scene PUBLIC $ $ 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 02818427a0..bab2761fc1 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 @@ -963,22 +963,11 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro * Requires a valid robot_model_ */ void initialize(); - /* helper function to create a RobotModel from a urdf/srdf. */ - static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model); - /* Helper functions for processing collision objects */ bool processCollisionObjectAdd(const moveit_msgs::msg::CollisionObject& object); bool processCollisionObjectRemove(const moveit_msgs::msg::CollisionObject& object); bool processCollisionObjectMove(const moveit_msgs::msg::CollisionObject& object); - /* For exporting and importing the planning scene */ - bool readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) const; - void writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) const; - - /** convert Pose msg to Eigen::Isometry, normalizing the quaternion part if necessary. */ - static void poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out); - MOVEIT_STRUCT_FORWARD(CollisionDetector); /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not nullptr */ diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/utilities.hpp b/moveit_core/planning_scene/include/moveit/planning_scene/utilities.hpp new file mode 100644 index 0000000000..a5b7d8fa01 --- /dev/null +++ b/moveit_core/planning_scene/include/moveit/planning_scene/utilities.hpp @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik 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: Andy Zelenak + Description: Helper functions related to the Planning Scene +*/ + +#pragma once + +#include + +namespace planning_scene +{ + +/** + * Helper function to create a RobotModel from a urdf/srdf. + * @param urdf_model The universal robot description + * @param srdf_model The semantic robot description + * @return nullptr on failure + */ +moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, + const srdf::ModelConstSharedPtr& srdf_model); + +/** + * convert Pose msg to Eigen::Isometry, normalizing the quaternion part if necessary. + * @param msg Input message + * @param out Output Eigen transform + */ +void poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out); + +/** \brief Read a pose from text */ +bool readPoseFromText(std::istream& in, Eigen::Isometry3d& pose); + +/** \brief Write a pose to text */ +void writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose); + +} // namespace planning_scene diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 2722ee6e61..47db6ff94e 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -163,17 +164,6 @@ void PlanningScene::initialize() allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } -// return nullptr on failure -moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, - const srdf::ModelConstSharedPtr& srdf_model) -{ - auto robot_model = std::make_shared(urdf_model, srdf_model); - if (!robot_model || !robot_model->getRootJoint()) - return moveit::core::RobotModelPtr(); - - return robot_model; -} - PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(parent) { if (!parent_) @@ -1078,30 +1068,6 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet } while (true); } -bool PlanningScene::readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) const -{ - double x, y, z, rx, ry, rz, rw; - if (!(in >> x >> y >> z)) - { - RCLCPP_ERROR(LOGGER, "Improperly formatted translation in scene geometry file"); - return false; - } - if (!(in >> rx >> ry >> rz >> rw)) - { - RCLCPP_ERROR(LOGGER, "Improperly formatted rotation in scene geometry file"); - return false; - } - pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz); - return true; -} - -void PlanningScene::writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) const -{ - out << pose.translation().x() << ' ' << pose.translation().y() << ' ' << pose.translation().z() << '\n'; - Eigen::Quaterniond r(pose.linear()); - out << r.x() << ' ' << r.y() << ' ' << r.z() << ' ' << r.w() << '\n'; -} - void PlanningScene::setCurrentState(const moveit_msgs::msg::RobotState& state) { // The attached bodies will be processed separately by processAttachedCollisionObjectMsgs @@ -1379,7 +1345,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id); Eigen::Isometry3d p; - PlanningScene::poseMsgToEigen(map.origin, p); + poseMsgToEigen(map.origin, p); p = t * p; world_->addToObject(OCTOMAP_NS, std::make_shared(om), p); } @@ -1497,7 +1463,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At Eigen::Isometry3d subframe_pose; for (std::size_t i = 0; i < object.object.subframe_poses.size(); ++i) { - PlanningScene::poseMsgToEigen(object.object.subframe_poses[i], subframe_pose); + poseMsgToEigen(object.object.subframe_poses[i], subframe_pose); std::string name = object.object.subframe_names[i]; subframe_poses[name] = subframe_pose; } @@ -1673,22 +1639,6 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::msg::CollisionO return false; } -void PlanningScene::poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out) -{ - Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z); - Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); - if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0)) - { - RCLCPP_WARN(LOGGER, "Empty quaternion found in pose message. Setting to neutral orientation."); - quaternion.setIdentity(); - } - else - { - quaternion.normalize(); - } - out = translation * quaternion; -} - bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::msg::CollisionObject& object, Eigen::Isometry3d& object_pose, std::vector& shapes, @@ -1714,7 +1664,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shapes.reserve(num_shapes); shape_poses.reserve(num_shapes); - PlanningScene::poseMsgToEigen(object.pose, object_pose); + poseMsgToEigen(object.pose, object_pose); bool switch_object_pose_and_shape_pose = false; if (num_shapes == 1) @@ -1731,7 +1681,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: if (!s) return; Eigen::Isometry3d pose; - PlanningScene::poseMsgToEigen(pose_msg, pose); + poseMsgToEigen(pose_msg, pose); if (!switch_object_pose_and_shape_pose) { shape_poses.emplace_back(std::move(pose)); @@ -1812,7 +1762,7 @@ bool PlanningScene::processCollisionObjectAdd(const moveit_msgs::msg::CollisionO Eigen::Isometry3d subframe_pose; for (std::size_t i = 0; i < object.subframe_poses.size(); ++i) { - PlanningScene::poseMsgToEigen(object.subframe_poses[i], subframe_pose); + poseMsgToEigen(object.subframe_poses[i], subframe_pose); std::string name = object.subframe_names[i]; subframes[name] = subframe_pose; } @@ -1854,7 +1804,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision const Eigen::Isometry3d& world_to_object_header_transform = getFrameTransform(object.header.frame_id); Eigen::Isometry3d header_to_pose_transform; - PlanningScene::poseMsgToEigen(object.pose, header_to_pose_transform); + poseMsgToEigen(object.pose, header_to_pose_transform); const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform; world_->setObjectPose(object.id, object_frame_transform); diff --git a/moveit_core/planning_scene/src/utilities.cpp b/moveit_core/planning_scene/src/utilities.cpp new file mode 100644 index 0000000000..ce8ca9de29 --- /dev/null +++ b/moveit_core/planning_scene/src/utilities.cpp @@ -0,0 +1,97 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik 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 PickNik 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: Andy Zelenak */ + +#include "moveit/planning_scene/utilities.hpp" + +namespace planning_scene +{ + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planning_scene.utilities"); +} + +moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, + const srdf::ModelConstSharedPtr& srdf_model) +{ + auto robot_model = std::make_shared(urdf_model, srdf_model); + if (!robot_model || !robot_model->getRootJoint()) + return moveit::core::RobotModelPtr(); + + return robot_model; +} + +void poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out) +{ + Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z); + Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); + if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0)) + { + RCLCPP_WARN(LOGGER, "Empty quaternion found in pose message. Setting to neutral orientation."); + quaternion.setIdentity(); + } + else + { + quaternion.normalize(); + } + out = translation * quaternion; +} + +bool readPoseFromText(std::istream& in, Eigen::Isometry3d& pose) +{ + double x, y, z, rx, ry, rz, rw; + if (!(in >> x >> y >> z)) + { + RCLCPP_ERROR(LOGGER, "Improperly formatted translation in scene geometry file"); + return false; + } + if (!(in >> rx >> ry >> rz >> rw)) + { + RCLCPP_ERROR(LOGGER, "Improperly formatted rotation in scene geometry file"); + return false; + } + pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz); + return true; +} + +void writePoseToText(std::ostream& out, const Eigen::Isometry3d& pose) +{ + out << pose.translation().x() << ' ' << pose.translation().y() << ' ' << pose.translation().z() << '\n'; + Eigen::Quaterniond r(pose.linear()); + out << r.x() << ' ' << r.y() << ' ' << r.z() << ' ' << r.w() << '\n'; +} + +} // namespace planning_scene