diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 37ab0f515b..9eb48ce44e 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -1,4 +1,6 @@ -add_library(moveit_planning_scene SHARED src/planning_scene.cpp) +add_library(moveit_planning_scene SHARED + src/planning_scene.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 9911a250d1..b7f81e375d 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 @@ -968,22 +968,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/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 2722ee6e61..ab92e0bf4a 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -59,6 +59,48 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planning_scene.p const std::string PlanningScene::OCTOMAP_NS = ""; const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)"; +namespace utilities +{ +/** + * 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) +{ + 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); + quaternion.normalize(); + out = translation * quaternion; +} + +/** \brief Read a pose from text */ +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; +} + +/** \brief Write a pose to text */ +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 utilities + class SceneTransforms : public moveit::core::Transforms { public: @@ -135,9 +177,12 @@ PlanningScene::PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, if (!srdf_model) throw moveit::ConstructException("The SRDF model cannot be nullptr"); - robot_model_ = createRobotModel(urdf_model, srdf_model); - if (!robot_model_) + robot_model_ = std::make_shared(urdf_model, srdf_model); + if (!robot_model_ || !robot_model_->getRootJoint()) + { + robot_model_ = nullptr; throw moveit::ConstructException("Could not create RobotModel"); + } initialize(); } @@ -163,17 +208,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_) @@ -917,7 +951,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const { out << "* " << id << '\n'; // New object start // Write object pose - writePoseToText(out, obj->pose_); + utilities::writePoseToText(out, obj->pose_); // Write shapes and shape poses out << obj->shapes_.size() << '\n'; // Number of shapes @@ -925,7 +959,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const { shapes::saveAsText(obj->shapes_[j].get(), out); // shape_poses_ is valid isometry by contract - writePoseToText(out, obj->shape_poses_[j]); + utilities::writePoseToText(out, obj->shape_poses_[j]); if (hasObjectColor(id)) { const std_msgs::msg::ColorRGBA& c = getObjectColor(id); @@ -939,8 +973,8 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const out << obj->subframe_poses_.size() << '\n'; // Number of subframes for (auto& pose_pair : obj->subframe_poses_) { - out << pose_pair.first << '\n'; // Subframe name - writePoseToText(out, pose_pair.second); // Subframe pose + out << pose_pair.first << '\n'; // Subframe name + utilities::writePoseToText(out, pose_pair.second); // Subframe pose } } } @@ -1000,7 +1034,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet // Read in object pose (added in the new scene format) pose.setIdentity(); - if (uses_new_scene_format && !readPoseFromText(in, pose)) + if (uses_new_scene_format && !utilities::readPoseFromText(in, pose)) { RCLCPP_ERROR(LOGGER, "Failed to read object pose from scene file"); return false; @@ -1019,7 +1053,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet RCLCPP_ERROR(LOGGER, "Failed to load shape from scene file"); return false; } - if (!readPoseFromText(in, pose)) + if (!utilities::readPoseFromText(in, pose)) { RCLCPP_ERROR(LOGGER, "Failed to read pose from scene file"); return false; @@ -1055,7 +1089,7 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet { std::string subframe_name; in >> subframe_name; - if (!readPoseFromText(in, pose)) + if (!utilities::readPoseFromText(in, pose)) { RCLCPP_ERROR(LOGGER, "Failed to read subframe pose from scene file"); return false; @@ -1078,30 +1112,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 +1389,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); + utilities::poseMsgToEigen(map.origin, p); p = t * p; world_->addToObject(OCTOMAP_NS, std::make_shared(om), p); } @@ -1497,7 +1507,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); + utilities::poseMsgToEigen(object.object.subframe_poses[i], subframe_pose); std::string name = object.object.subframe_names[i]; subframe_poses[name] = subframe_pose; } @@ -1673,22 +1683,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 +1708,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shapes.reserve(num_shapes); shape_poses.reserve(num_shapes); - PlanningScene::poseMsgToEigen(object.pose, object_pose); + utilities::poseMsgToEigen(object.pose, object_pose); bool switch_object_pose_and_shape_pose = false; if (num_shapes == 1) @@ -1731,7 +1725,7 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: if (!s) return; Eigen::Isometry3d pose; - PlanningScene::poseMsgToEigen(pose_msg, pose); + utilities::poseMsgToEigen(pose_msg, pose); if (!switch_object_pose_and_shape_pose) { shape_poses.emplace_back(std::move(pose)); @@ -1812,7 +1806,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); + utilities::poseMsgToEigen(object.subframe_poses[i], subframe_pose); std::string name = object.subframe_names[i]; subframes[name] = subframe_pose; } @@ -1854,7 +1848,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); + utilities::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);