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.

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

MoveIt! tf2 migration #830

Merged
merged 6 commits into from May 18, 2018

Conversation

Projects
None yet
7 participants
@IanTheEngineer
Copy link
Member

IanTheEngineer commented Apr 9, 2018

This will resolve #745 and complete a checkbox for Release to Melodic.

This is the migration from tf to tf2 required for ROS2. It depends on ros/geometry2#292 for a handful of additional conversions. It breaks API compatibility, so it can only be merged into the melodic-devel branch.

This PR depends on the following upstream API changes for Melodic:
ros/geometry#163 (Exposing tf2_ros::Buffer object from TransformListener)
ros/geometry2#292 (tf2 Type conversions)
ros/geometry2#294 (Additional tf2 Type conversions)
ros-visualization/rviz#1236 (Exposing tf2 without ROS deprecation warnings)

Highlights:

  • All type conversions now depend on geometry2 ROS packages, rather than geometry
  • Removed all boost::shared_ptr<tf::TransformListener> from the API, and replaced them with std::shared_ptr<tf2_ros::Buffer>'s. I figured boost was only kept to preserve API, but please correct me if that is a bad assumption.
  • Piped tf2_ros::Buffer's everywhere where tf::TransformListeners were used

Things left TODO:

  • Finish converting moveit_ros/robot_interaction/src/robot_interaction.cpp
  • Finish converting moveit_ros/perception/mesh_filter/src/transform_provider.cpp
  • Figure out a better method for retrieving RViz' internal tf2_ros::Buffer instance, as RViz APi only gives access to the boost::shared_ptr<tf::TransformListener> instance:
    ros-visualization/rviz#1215
  • Fix one rather large bug likely caused by my work around to the above point: when wiggling around the interactive markers in RViz, I can periodically get in a state where the marker detaches from the robot's end effector, and this error prints out:
[ERROR] [1523131812.122517887]: Cannot transform from frame 'base' to frame '/base' (no TF instance provided)
[ERROR] [1523131812.148109554]: Cannot transform from frame 'base' to frame '/base' (no TF instance provided)
[ERROR] [1523131812.179335945]: Cannot transform from frame 'base' to frame '/base' (no TF instance provided)

My solution was to just pass in a null Buffer to the move_group when we would have used RViz's TransformListener. This almost seems to work, aside for the bug.

Checklist

  • Code is (sort-of) auto formatted using clang-format, but I ignored some reformats that looked strange. I'll run it again after review changes.
  • TBD: Extended the tutorials / documentation, if necessary reference
  • TBD: Created tests, which fail without this PR reference
  • As this breaks API/ABI, this PR is melodic-devel only

@IanTheEngineer IanTheEngineer added this to the ROS Melodic Release milestone Apr 9, 2018

@IanTheEngineer IanTheEngineer requested a review from tfoote Apr 9, 2018

@IanTheEngineer IanTheEngineer referenced this pull request Apr 9, 2018

Merged

tf2 migration #24

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented Apr 9, 2018

travis-ci won't be happy as this PR depends on ros/geometry2#292. I'm open to recommendations on how to remedy that, if necessary.

@davetcoleman davetcoleman referenced this pull request Apr 9, 2018

Closed

Release to ROS Melodic #806

19 of 25 tasks complete
@v4hn

This comment has been minimized.

Copy link
Member

v4hn commented Apr 10, 2018

@davetcoleman davetcoleman changed the base branch from kinetic-devel to melodic-devel Apr 11, 2018

@davetcoleman
Copy link
Member

davetcoleman left a comment

Huge PR!!

tf::pointEigenToMsg(contact.pos, msg.position);
tf::vectorEigenToMsg(contact.normal, msg.normal);
msg.position = tf2::toMsg(contact.pos);
// FIXME: there really should be a conversion for

This comment has been minimized.

@davetcoleman

davetcoleman Apr 11, 2018

Member

// TODO(IanTheEngineer)

@@ -275,8 +275,12 @@ void robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::Robot
trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
for (std::size_t j = 0; j < mdof.size(); ++j)
{
tf::transformEigenToMsg(waypoints_[i]->getJointTransform(mdof[j]),
trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
// FIXME: there should be a

This comment has been minimized.

@davetcoleman
@@ -69,8 +67,6 @@ class CHOMPPlanningContext : public planning_interface::PlanningContext
private:
CHOMPInterfacePtr chomp_interface_;
moveit::core::RobotModelConstPtr robot_model_;

boost::shared_ptr<tf::TransformListener> tf_;

This comment has been minimized.

@davetcoleman
context_->planning_scene_monitor_->getTFClient()->getLatestCommonTime(pose_msg.header.frame_id, target_frame,
common_time, &error);
int pf = context_->planning_scene_monitor_->getTFClient()->_lookupFrameNumber(pose_msg.header.frame_id);
int cf = context_->planning_scene_monitor_->getTFClient()->_lookupFrameNumber(target_frame);

This comment has been minimized.

@davetcoleman

davetcoleman Apr 11, 2018

Member

better var names

@@ -88,7 +90,9 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)

bool PointCloudOctomapUpdater::initialize()
{
tf_ = monitor_->getTFClient();
tf_buffer_.reset(new tf2_ros::Buffer());
// FIXME(imcmahon) should the TransformListener use the private_nh_, root_nh_ or create it's own NodeHandle?

This comment has been minimized.

@davetcoleman
// FIXME!(imcmahon) this forces the Planning Scene Monitor to allocate a new tf2_ros::Buffer
// and tf2_ros::TransformListener on each invocation. These instances are properly deleted on exit,
// but it would be better to remove the null shared pointer once tf2_ros::Buffer is exposed from
// RViz with something like context_->getFrameManager()->getTFClientPtr()

This comment has been minimized.

@davetcoleman
@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented Apr 20, 2018

Commit 2da2c8d depends on ros-visualization/rviz#1224, and resolves the two main issues when dealing with transforms from RViz: the crashes no longer manifest.

move_group_.reset(new moveit::planning_interface::MoveGroupInterface(
opt, std::shared_ptr<tf2_ros::Buffer>(), ros::WallDuration(30, 0)));
opt, context_->getFrameManager()->getTFBufferPtr(), ros::WallDuration(30, 0)));

This comment has been minimized.

@IanTheEngineer

@IanTheEngineer IanTheEngineer force-pushed the IanTheEngineer:tf2_migration branch 3 times, most recently from 33e4c48 to 1ea1e00 Apr 22, 2018

@tfoote

This comment has been minimized.

Copy link
Member

tfoote commented Apr 27, 2018

@IanTheEngineer let me know when you've pushed everything to geometry2 and I can get a new release out in melodic

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented Apr 30, 2018

@tfoote all the changes that I need are now merged into geometry2, so I think you're good to go in releasing to Melodic. Thanks!

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 3, 2018

@davetcoleman and @v4hn I think this PR should build cleanly now that the dependencies have been merged into Melodic. However, the pull request builder is using Kinetic. How do I update the builder to use the right distro?

@IanTheEngineer IanTheEngineer referenced this pull request May 3, 2018

Closed

upgrade to tf2 #745

@davetcoleman

This comment has been minimized.

Copy link
Member

davetcoleman commented May 3, 2018

For now I think we'll need to go without the pull request builder, I have not had time to set this up yet.

Can you remove this from the first description if its ready?

Note: This PR is not ready for merge, but ready for discussion and feedback. I will rebase the 20+ commits down to a small, sensible number.

You have an unfinished TODO in the first description:

Finish converting moveit_ros/perception/mesh_filter/src/transform_provider.cpp

These are ready?

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 3, 2018

Can you remove this from the first description if its ready?

Note: This PR is not ready for merge, but ready for discussion and feedback. I will rebase the 20+ commits down to a small, sensible number.

I can rebase this down now, but I typically do this right before a merge, after the reviews are made and feedback is incorporated. Keeping them separate helps in viewing the history of the PR, but I admit 40 commits is an unwieldy number.

You have an unfinished TODO in the first description:

Finish converting moveit_ros/perception/mesh_filter/src/transform_provider.cpp

These are ready?

Yep. All of the code is ready. I'll update the checkbox accordingly.

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented May 3, 2018

Please wait with squashing. I'm currently reviewing...

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 3, 2018

@rhaschke gotcha. I'll hold off on the squash until I have sufficient reviews.

@rhaschke
Copy link
Contributor

rhaschke left a comment

Generally I approve these changes. @IanTheEngineer, thanks a lot for this great work! Some minor remarks inline.

@tfoote This PR uses the methods _addTransformsChangedListener and _getLatestCommonTime of tf2::BufferCore, which are declared "not to be used anymore". What are the recommended alternatives?

try
{
tf_->transformPose(frame_id_, input_pose, out_pose);
// TODO: check logic here - which global collision body's transform should be used?
input_transform.setData(robot_state->getAttachedBody(contextIt->second->frame_id_)->getGlobalCollisionBodyTransforms()[0]);

This comment has been minimized.

@rhaschke

rhaschke May 3, 2018

Contributor

Why not keep the previous code here?
input_transform.setData(robot_state->getLinkState(contextIt->second->frame_id_)->getGlobalCollisionBodyTransform());

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 4, 2018

Author Member

Well, transfom_provider is never actually compiled, and if it were to be compiled, it would introduce a circular dependency between moveit_ros_perception and moveit_ros_planning, due to its use of planning_scene_monitor.

Additionally, it turns out the functiongetGlobalCollisionBodyTransform doesn't exist anywhere in the codebase. In order to make sure all of my other updates to this file were correct (tf2 message & type conversions, etc), I forced the compile of this file, which fails on this line. I put a band-aid on it to allow compiling, but it's likely not correct. I'm not sure what the proper procedure should be with this dead code.

if (!s.tf_)
s.tf_.reset(new tf::TransformListener());
return s.tf_;
if (!s.tf_buffer_ || !s.tf_listener_)

This comment has been minimized.

@rhaschke

rhaschke May 3, 2018

Contributor

Doesn't it suffice to check for listener? Both, buffer and listener, are always created together?

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 17, 2018

Author Member

That is correct, both the buffer and the listener should be created together, and therefore just checking the for the listener should suffice. I structured this as such out of an abundance of caution in case the buffer is somehow deleted: the SharedStorage is a struct, making tf_buffer_ a public method. However, you are right that this logic can be simplified.

return s.tf_;
if (!s.tf_buffer_ || !s.tf_listener_)
{
if(s.tf_listener_)

This comment has been minimized.

@rhaschke

rhaschke May 3, 2018

Contributor

No need for this. Buffer is a shared_ptr anyway.

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 17, 2018

Author Member

That's true, the buffer will continue to persist.

tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
tfScalar pitch, roll, yaw;
ptf.getRPY(roll, pitch, yaw);
geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));

This comment has been minimized.

@rhaschke

rhaschke May 3, 2018

Contributor

YPR can be directly computed from Eigen:

// yaw, pitch, roll corresponds to rotations about z, y, x axes, i.e. indeces 2, 1, 0 - in that order
Eigen::Map<Eigen::Vector3d>(&result[0]) = current_state->getGlobalLinkTransform(lm).rotation().eulerAngles(2, 1, 0)

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 4, 2018

Author Member

Well that's nifty. I'll update with your Eigen method.

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 7, 2018

Author Member

@rhaschke wouldn't this line store the values in result as [yaw, pitch, roll] when result is expecting [roll, pitch, yaw]?

This comment has been minimized.

@rhaschke

rhaschke May 7, 2018

Contributor

Damn. Haven't seen that. Indeed, this would required to switch roll and yaw or to reverse() the result of eulerAngles():
Eigen::Map<Eigen::Vector3d>(&result[0]) = current_state->getGlobalLinkTransform(lm).rotation().eulerAngles(2, 1, 0).reverse()

However, I'm completely fine, if you decide to keep your code for better readibility.

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 17, 2018

Author Member

I'll use your updated Eigen method. I think a comment will suffice for readability.

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 17, 2018

Author Member

After fiddling with this for a bit, I think I will keep the original tf2 conversion code.

@tfoote

tfoote approved these changes May 4, 2018

Copy link
Member

tfoote left a comment

Reading this through it looks good. @rhaschke had a few suggestions that looked good too. I'm not likely to have a chance to run through again so +1

@IanTheEngineer IanTheEngineer referenced this pull request May 15, 2018

Closed

Remove boost shared ptrs #896

0 of 2 tasks complete

@IanTheEngineer IanTheEngineer force-pushed the IanTheEngineer:tf2_migration branch from 6864801 to 6de8b83 May 16, 2018

@rhaschke
Copy link
Contributor

rhaschke left a comment

@IanTheEngineer In view of the upcoming Melodic release, I'm fine to skip the replacement of deprecated methods _addTransformsChangedListener and _getLatestCommonTime for now ;-)
However, you should check for Travis' complaints.

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 17, 2018

@rhaschke I'm actually pretty close to fixing _getLatestCommonTime... I'll have some changes up soon.

IanTheEngineer and others added some commits May 17, 2018

tf2 migration, resolves issue #745
- All type conversions now depend on geometry2 ROS packages, rather
  than geometry (see ros/geometry2#292 and
  ros/geometry2#294 for details of the new
  conversions)
- Removes all boost::shared_ptr<tf::TransformListener> from the API,
  and replaced them with std::shared_ptr<tf2_ros::Buffer>'s
- Piped tf2_ros::Buffer's everywhere where tf::TransformListeners were
  used
- Utilizes the new tf2 API in the tf::Transformer library to access
  the internal tf2::Buffer object used by RViz
  (see ros/geometry#163 for details of the
  new API)
- Removes the prepending of forward slashes ('/') for Transforms frames
  as this is deprecated in tf2

@IanTheEngineer IanTheEngineer force-pushed the IanTheEngineer:tf2_migration branch from 6de8b83 to c09d6bb May 17, 2018

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 17, 2018

I squashed most of the history of this PR, but it's still not quite ready for merging. I still have to

  • re-run clang cleanup
  • finish addressing @rhaschke 's review comments

As it would require fully grokking how TransformableRequest interface works, I am tempted to hold off on fixing the deprecated _addTransformsChangedListener for now, in the interest of getting a release out. I can solve that in a separate PR later on.

@IanTheEngineer IanTheEngineer force-pushed the IanTheEngineer:tf2_migration branch from 881ee93 to 8c3f2d7 May 17, 2018

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 17, 2018

At this point, I think the only reason Travis is failing is that it's attempting to build with kinetic.

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 17, 2018

@rhaschke thanks for the fix - seems I incorrectly melded that file.

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented May 17, 2018

I'm currently looking into Travis / docker to fix our CI for melodic.

const ros::WallDuration& wait_for_servers = ros::WallDuration());
MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf,
MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,

This comment has been minimized.

@bmagyar

bmagyar May 17, 2018

Member

Isn't it time to drop everything marked with MOVEIT_DEPRECATED instead of migrating them?

This comment has been minimized.

@rhaschke

rhaschke May 17, 2018

Contributor

Indeed, we should have a check for deprecated stuff. But we should carefully check, when and why we deprecated. I would only remove stuff that was deprecated in Indigo.

This comment has been minimized.

@bmagyar

bmagyar May 18, 2018

Member

The point of keeping them with the deprecated flag was that they remain unchanged. If we keep updating their signature, the original functions are gone regardless.

I think the two scallywags below should be removed instead of updating them even though they were not deprecated already in Indigo.

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 18, 2018

Author Member

I have no preference on updating or removing. However, for the purposes of preventing this PR from becoming too big and unwieldy, I tried to use a light touch in only updating what I absolutely had to update for tf2 to work.
I think another PR is in order for removing all MOVEIT_DEPRECATED functions from the Lunar/Kinetic release for Melodic.

This comment has been minimized.

@IanTheEngineer

IanTheEngineer May 18, 2018

Author Member

... which I see you just recommended in #902 (comment). Appears we're on the same page @bmagyar :)

@rhaschke

This comment has been minimized.

Copy link
Contributor

rhaschke commented May 18, 2018

I approved that this compiles for Melodic and I'm going to squash-merge now.

@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 18, 2018

I'm fine with that, all of the changes are now in. Would you prefer that I squash everything manually in order to preserve the (hopefully informative) commit message?

@rhaschke rhaschke merged commit 736251d into ros-planning:melodic-devel May 18, 2018

1 check failed

continuous-integration/travis-ci/pr The Travis CI build failed
Details
@IanTheEngineer

This comment has been minimized.

Copy link
Member Author

IanTheEngineer commented May 18, 2018

Nevermind! Looks like you handled it beautifully. Thanks @rhaschke! And thanks to everyone else for the reviews and feedback. It was a nice birthday present to have this merged today 🎂 😄

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.