Skip to content

Commit

Permalink
Move stateless PlanningScene helper functions out of the class (#2025)
Browse files Browse the repository at this point in the history
  • Loading branch information
@AndyZe
AndyZe committed Apr 4, 2023
1 parent 9d5249c commit dd2c409
Show file tree
Hide file tree
Showing 3 changed files with 63 additions and 78 deletions.
4 changes: 3 additions & 1 deletion moveit_core/planning_scene/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/moveit_core>
Expand Down
11 changes: 0 additions & 11 deletions moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down
126 changes: 60 additions & 66 deletions moveit_core/planning_scene/src/planning_scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,48 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planning_scene.p
const std::string PlanningScene::OCTOMAP_NS = "<octomap>";
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:
Expand Down Expand Up @@ -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<moveit::core::RobotModel>(urdf_model, srdf_model);
if (!robot_model_ || !robot_model_->getRootJoint())
{
robot_model_ = nullptr;
throw moveit::ConstructException("Could not create RobotModel");
}

initialize();
}
Expand All @@ -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<moveit::core::RobotModel>(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_)
Expand Down Expand Up @@ -917,15 +951,15 @@ 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
for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
{
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);
Expand All @@ -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
}
}
}
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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<const shapes::OcTree>(om), p);
}
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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::ShapeConstPtr>& shapes,
Expand All @@ -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)
Expand All @@ -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));
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit dd2c409

Please sign in to comment.