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

Port transforms submodule of moveit_core of ROS 2 #12

Merged
merged 5 commits into from
Mar 15, 2019
Merged

Port transforms submodule of moveit_core of ROS 2 #12

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
3a14080
Port transforms submodule of moveit_core of ROS 2
vmayoral Feb 19, 2019
57814c6
transforms submodule, add tests
vmayoral Feb 24, 2019
6ca40b1
Cleanup transforms submodule meta-files
vmayoral Feb 24, 2019
94ebbcc
moveit_core, transforms submodule, back to Isometry3d
vmayoral Mar 3, 2019
7fa3dfa
transforms, fix logging
vmayoral Mar 4, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 8 additions & 5 deletions moveit_core/transforms/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,16 @@ set(MOVEIT_LIB_NAME moveit_transforms)
add_library(${MOVEIT_LIB_NAME} src/transforms.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${MOVEIT_LIB_NAME} ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
ament_target_dependencies(${MOVEIT_LIB_NAME}
tf2_eigen)


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)
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

# Unit tests
if(CATKIN_ENABLE_TESTING)
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
Expand Down
25 changes: 13 additions & 12 deletions moveit_core/transforms/include/moveit/transforms/transforms.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,12 @@
#ifndef MOVEIT_TRANSFORMS_
#define MOVEIT_TRANSFORMS_

#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It may make sense to make these changes repo wide with a find-replace all. Otherwise we will be manually making 462 changes across 91 files for just geometry_msgs:: and 23 times across 19 files for #include <geometry_msgs/msg/

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

strong yes!

#include <geometry_msgs/msg/pose.hpp>
#include <Eigen/Geometry>
#include <boost/noncopyable.hpp>
#include <moveit/macros/class_forward.h>
#include <map>

namespace moveit
{
Expand All @@ -51,8 +52,8 @@ MOVEIT_CLASS_FORWARD(Transforms);

/// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning
/// frame
typedef std::map<std::string, Eigen::Isometry3d, std::less<std::string>,
Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d> > >
typedef std::map<std::string, Eigen::Affine3d, std::less<std::string>,
Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d> > >
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This was a big change we made to moveit in Dec, why is it being reverted here?

moveit/moveit#1096

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Partially described at https://github.com/AcutronicRobotics/moveit2-original/pull/9. See https://github.com/ros2/geometry2/blob/ros2/tf2_eigen/include/tf2_eigen/tf2_eigen.h#L46.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Going back to Affine (or cluttering the history with hundreds of lines of unnecessary revert/counter-revert) is not an option. If you found a problem with ros2's geometry2-package, this should be fixed there (and this repository should probably require the patched version until then.)

I would hope upstream is very open to fixes in their interfaces if we need them.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

agreed, we definitely should not revert back to Affine3d, that's a performance hit

i just looked into the difference between melodic-devel and ros2 branch and i don't see any changes in functions for in tf2_eigen.h. why does this require changing for moveit2?

Copy link
Contributor Author

@vmayoral vmayoral Feb 24, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@davetcoleman and @v4hn, have a look at the ROS 2 geometry2 package as indicated above. For further details:

There's been a switch to Affine3d. Maybe @clalancette could advice if you are willing to accept a change reverting to Isometry3d? We should try to reach a compromise before producing any further changes.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@vmayoral: the switch was actually to Isometry3D (as that makes much more sense). There's nothing to revert.

See:

It seems ros2/geometry2 just hasn't been kept up-to-date (example of ROS1 - ROS2 community/maintenance split?).

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks @gavanderhoorn for clarifying.

It seems ros2/geometry2 just hasn't been kept up-to-date (example of ROS1 - ROS2 community/maintenance split?).

I guess so.

@clalancette, how shall we proceed about this? Shall I book some time next week and provide a PR to adjust things in geometry2 for ROS2 or do you want to take care of this yourself? We can then re-adjust things here afterwards.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is the PR that needs to be forward ported: ros/geometry2#335

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See 94ebbcc. I believe we should be fine with this, right @davetcoleman?

FixedTransformsMap;

/** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for
Expand Down Expand Up @@ -87,37 +88,37 @@ class Transforms : private boost::noncopyable

/**
* @brief Return all the transforms
* @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame)
* @return A map from string names of frames to corresponding Eigen::Affine3d (w.r.t the planning frame)
*/
const FixedTransformsMap& getAllTransforms() const;

/**
* @brief Get a vector of all the transforms as ROS messages
* @param transforms The output transforms
*/
void copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const;
void copyTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms) const;

/**
* @brief Set a transform in the transform tree (adding it if necessary)
* @param t The input transform (w.r.t the target frame)
* @param from_frame The frame for which the input transform is specified
*/
void setTransform(const Eigen::Isometry3d& t, const std::string& from_frame);
void setTransform(const Eigen::Affine3d& t, const std::string& from_frame);

/**
* @brief Set a transform in the transform tree (adding it if necessary)
* @param transform The input transform (the frame_id must match the target frame)
*/
void setTransform(const geometry_msgs::TransformStamped& transform);
void setTransform(const geometry_msgs::msg::TransformStamped& transform);

/**
* @brief Set a transform in the transform tree (adding it if necessary)
* @param transform The input transforms (the frame_id must match the target frame)
*/
void setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms);
void setTransforms(const std::vector<geometry_msgs::msg::TransformStamped>& transforms);

/**
* @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the
* @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Affine3d (w.r.t the
* planning frame)
*/
void setAllTransforms(const FixedTransformsMap& transforms);
Expand Down Expand Up @@ -170,7 +171,7 @@ class Transforms : private boost::noncopyable
* @param t_in The input pose (in from_frame)
* @param t_out The resultant (transformed) pose
*/
void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const
void transformPose(const std::string& from_frame, const Eigen::Affine3d& t_in, Eigen::Affine3d& t_out) const
{
t_out = getTransform(from_frame) * t_in;
}
Expand All @@ -192,7 +193,7 @@ class Transforms : private boost::noncopyable
* @param from_frame The string id of the frame for which the transform is being computed
* @return The required transform
*/
virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const;
virtual const Eigen::Affine3d& getTransform(const std::string& from_frame) const;

protected:
std::string target_frame_;
Expand Down
18 changes: 9 additions & 9 deletions moveit_core/transforms/src/transforms.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@
#include <moveit/transforms/transforms.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/algorithm/string/trim.hpp>
#include <ros/console.h>
#include <moveit/logging/logging.h>

namespace moveit
{
Expand All @@ -50,7 +50,7 @@ Transforms::Transforms(const std::string& target_frame) : target_frame_(target_f
ROS_ERROR_NAMED("transforms", "The target frame for MoveIt! Transforms cannot be empty.");
else
{
transforms_map_[target_frame_] = Eigen::Isometry3d::Identity();
transforms_map_[target_frame_] = Eigen::Affine3d::Identity();
}
}

Expand Down Expand Up @@ -86,7 +86,7 @@ bool Transforms::isFixedFrame(const std::string& frame) const
return transforms_map_.find(frame) != transforms_map_.end();
}

const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame) const
const Eigen::Affine3d& Transforms::getTransform(const std::string& from_frame) const
{
if (!from_frame.empty())
{
Expand All @@ -100,7 +100,7 @@ const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame)
from_frame.c_str(), target_frame_.c_str());

// return identity
static const Eigen::Isometry3d IDENTITY = Eigen::Isometry3d::Identity();
static const Eigen::Affine3d IDENTITY = Eigen::Affine3d::Identity();
return IDENTITY;
}

Expand All @@ -112,19 +112,19 @@ bool Transforms::canTransform(const std::string& from_frame) const
return transforms_map_.find(from_frame) != transforms_map_.end();
}

void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame)
void Transforms::setTransform(const Eigen::Affine3d& t, const std::string& from_frame)
{
if (from_frame.empty())
ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name");
else
transforms_map_[from_frame] = t;
}

void Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
void Transforms::setTransform(const geometry_msgs::msg::TransformStamped& transform)
{
if (sameFrame(transform.child_frame_id, target_frame_))
{
Eigen::Isometry3d t = tf2::transformToEigen(transform.transform);
Eigen::Affine3d t = tf2::transformToEigen(transform);
setTransform(t, transform.header.frame_id);
}
else
Expand All @@ -134,13 +134,13 @@ void Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
}
}

void Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms)
void Transforms::setTransforms(const std::vector<geometry_msgs::msg::TransformStamped>& transforms)
{
for (std::size_t i = 0; i < transforms.size(); ++i)
setTransform(transforms[i]);
}

void Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const
void Transforms::copyTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms) const
{
transforms.resize(transforms_map_.size());
std::size_t i = 0;
Expand Down