Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Move stateless PlanningScene helper functions out of the class #2025

Merged
merged 5 commits into from Apr 4, 2023
Merged

Move stateless PlanningScene helper functions out of the class #2025

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a6a1ccc
Move PlanningScene helper functions to new cpp file
AndyZe Mar 19, 2023
22a5fbc
Delete createRobotModel(). Minor cleanup.
AndyZe Mar 23, 2023
ed89fe6
Move the utilities to planning_scene.cpp
AndyZe Mar 23, 2023
ccf26db
Merge branch 'main' into andyz/ps_utilities
AndyZe Mar 24, 2023
c4207bf
Merge branch 'main' into andyz/ps_utilities
AndyZe Apr 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 3 additions & 1 deletion moveit_core/planning_scene/CMakeLists.txt
View file
Open in desktop
@@ -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
Open in desktop
Expand Up @@ -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 */
Expand Down
126 changes: 60 additions & 66 deletions moveit_core/planning_scene/src/planning_scene.cpp
View file
Open in desktop
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)
henningkayser marked this conversation as resolved.
Show resolved Hide resolved
{
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