Create a transform subscribers to enable virtual joints #310

JafarAbdi · 2020-11-18T21:36:11Z

Description

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

codecov · 2020-11-18T22:30:27Z

Codecov Report

Merging #310 (578664e) into main (4f5bacd) will increase coverage by 0.14%.
The diff coverage is 81.58%.

@@            Coverage Diff             @@
##             main     #310      +/-   ##
==========================================
+ Coverage   53.98%   54.12%   +0.14%     
==========================================
  Files         190      190              
  Lines       20002    20034      +32     
==========================================
+ Hits        10796    10841      +45     
+ Misses       9206     9193      -13

Impacted Files	Coverage Δ
...eit/planning_scene_monitor/current_state_monitor.h	`77.78% <ø> (ø)`
...anning_scene_monitor/src/current_state_monitor.cpp	`74.18% <72.00%> (+9.23%)`	⬆️
...or/src/current_state_monitor_middleware_handle.cpp	`85.72% <100.00%> (+12.39%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 4f5bacd...578664e. Read the comment docs.

henningkayser · 2020-11-19T19:23:51Z

moveit_ros/move_group/src/move_group.cpp

@@ -200,7 +200,6 @@ int main(int argc, char** argv)

  std::shared_ptr<tf2_ros::Buffer> tf_buffer =
      std::make_shared<tf2_ros::Buffer>(nh->get_clock(), tf2::durationFromSec(10.0));
-  std::shared_ptr<tf2_ros::TransformListener> tfl = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);


We shouldn't rely on the CurrentStateMonitor to fill the tf_buffer. The TransformListener is required for the PlanningSceneMonitor to work without it.

I think this comment is obsolete now, since parallel functionality was implemented through subscribers on the /tf topic.

How is this comment obsolete? The subscribers are initialized inside the CSM so unless we call PSM::startStateMonitor() there won't be any listeners updating the buffer. A reasonable workaround would be to move the transform subscribers into the PSM and to call CSM::updateMultiDofJoints from there. What do you think?

We do always call startStateMonitor when constructing move_group & moveit_cpp

henningkayser · 2020-11-19T19:28:28Z

moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp

@@ -61,7 +60,6 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options
 {
  if (!tf_buffer_)
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);


Here the same, we should run a TransformListener independent of the CSM.

Same with this comment.

See my previous comment

henningkayser · 2020-11-20T10:54:26Z

I thought about this and I don't think we should merge this solution. We should have an independent TransformListener running at all times. If that's not possible, we might implement our own listener that supports callbacks.

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

schornakj · 2021-04-05T01:56:22Z

moveit_ros/move_group/src/move_group.cpp

@@ -200,7 +200,6 @@ int main(int argc, char** argv)

  std::shared_ptr<tf2_ros::Buffer> tf_buffer =
      std::make_shared<tf2_ros::Buffer>(nh->get_clock(), tf2::durationFromSec(10.0));
-  std::shared_ptr<tf2_ros::TransformListener> tfl = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);


I think this comment is obsolete now, since parallel functionality was implemented through subscribers on the /tf topic.

schornakj · 2021-04-05T01:56:39Z

moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp

@@ -61,7 +60,6 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options
 {
  if (!tf_buffer_)
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);


Same with this comment.

DLu · 2021-05-04T15:14:39Z

@JafarAbdi Any thoughts on addressing @schornakj 's comments?

henningkayser

Sorry for letting the reviews pend for so long. I think this PR is really getting there but the original complaint doesn't seem to be fixed, which is that the PSM doesn't update TF if the CSM is not initialized (please correct me if I'm wrong). This could for instance break the planning scene display as that one doesn't listen for joint states explicitly. The callback implementation looks good to me on its own (argree, no class required) but please consider moving them into the PSM and updating the transforms from there (see comment).

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

henningkayser · 2021-06-10T12:32:22Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

+                   transform.child_frame_id.c_str(), transform.header.frame_id.c_str(), temp.c_str());
+    }
+  }
+  updateMultiDofJoints();


isn't this redundant in case of a transform exception?

What if the exception happened after adding few transforms to the buffer .?

...lanning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h

henningkayser · 2021-06-10T12:42:11Z

moveit_ros/move_group/src/move_group.cpp

@@ -200,7 +200,6 @@ int main(int argc, char** argv)

  std::shared_ptr<tf2_ros::Buffer> tf_buffer =
      std::make_shared<tf2_ros::Buffer>(nh->get_clock(), tf2::durationFromSec(10.0));
-  std::shared_ptr<tf2_ros::TransformListener> tfl = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);


How is this comment obsolete? The subscribers are initialized inside the CSM so unless we call PSM::startStateMonitor() there won't be any listeners updating the buffer. A reasonable workaround would be to move the transform subscribers into the PSM and to call CSM::updateMultiDofJoints from there. What do you think?

JafarAbdi · 2021-08-11T23:07:47Z

I think this PR is ready to be merged, regarding having an independent transform listener for PSM I didn't change the behavior from ROS1, the user have to provide it themselves unless a current state monitor is also enabled in that case a warning message will be print telling that the transforms will be added by both transform listener & current state monitor's callbacks

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

nbbrooks · 2021-08-13T18:23:40Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

@@ -401,8 +408,11 @@ void CurrentStateMonitor::tfCallback()
        continue;
      }

+      if (auto joint_time_it = joint_time_.find(joint); joint_time_it == joint_time_.end())
+        joint_time_.emplace(joint, rclcpp::Time(0, 0, RCL_ROS_TIME));
+
      // allow update if time is more recent or if it is a static transform (time = 0)


Suggested change

// allow update if time is more recent or if it is a static transform (time = 0)

// skip if time is not more recent and is not a static transform (time = 0)

Comments should use the same logic as the conditionals, don't make the person parsing it invert conditions.

…ing csm

…itor

…onitor.cpp Co-authored-by: Nathan Brooks <nbbrooks@gmail.com>

henningkayser

LGTM! Also, thanks for the tests @JafarAbdi. This fix is good for now, but I think this PR exposes serious flaws in the PSM API that should be addressed.

Using an external TF listener is discouraged now in case CSM is used since transforms callbacks would be duplicate. This should be documented and ideally checked for inside the PSM constructor. If the CSM is not used, an external listener is strictly required to keep transforms updated. For the PSM it means that it's highly questionable if tf_buffer receives updated transforms or not. Obviously, some of this confusion was already present in ROS 1, but this PR makes it just a little bit more confusing.

One way to guarantee that PSM has a valid listener would be to:

Remove/Deprecate tf_buffer from PSM constructors
Initialize a private tf_listener inside PSM, possibly using startTFListener() to make this optional
Stop tf_listener with startStateMonitor(), restart it with stopStateMonitor()

(this is definitely out of scope of this PR)

tylerjw

Thank you for adding this. I generally agree with Henning that this part of the code needs a closer look to make the correct use of the API more obvious.

henningkayser reviewed Nov 19, 2020

View reviewed changes

henningkayser added this to In progress in Foxy 2.1.3 - May 21 via automation Mar 30, 2021

JafarAbdi force-pushed the pr-virtual_joints branch from 7afc314 to d0cd946 Compare March 30, 2021 18:02

henningkayser assigned schornakj Mar 31, 2021

moveit deleted a comment from schornakj Mar 31, 2021

JafarAbdi force-pushed the pr-virtual_joints branch from d0cd946 to 534cc10 Compare April 1, 2021 17:23

schornakj reviewed Apr 5, 2021

View reviewed changes

henningkayser removed this from In progress in Foxy 2.1.3 - May 21 May 22, 2021

henningkayser added this to In progress in Foxy/Galactic/Rolling 2.2 - June 30 May 22, 2021

JafarAbdi requested a review from henningkayser June 9, 2021 20:55

henningkayser reviewed Jun 10, 2021

View reviewed changes

henningkayser mentioned this pull request Jun 11, 2021

Port to ros2 moveit/moveit_visual_tools#92

Merged

henningkayser unassigned schornakj Jun 15, 2021

henningkayser removed this from In progress in Foxy/Galactic/Rolling 2.2 - June 30 Jul 12, 2021

henningkayser added this to Review in progress in Galactic/Rolling - 2.3.0 - September 10 Jul 12, 2021

JafarAbdi force-pushed the pr-virtual_joints branch 2 times, most recently from 0eb4492 to 16814bb Compare July 29, 2021 15:23

JafarAbdi force-pushed the pr-virtual_joints branch 2 times, most recently from a0b6b0c to 9f3be3e Compare August 11, 2021 22:55

JafarAbdi requested a review from henningkayser August 11, 2021 23:07

nbbrooks reviewed Aug 13, 2021

View reviewed changes

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Show resolved Hide resolved

nbbrooks reviewed Aug 13, 2021

View reviewed changes

JafarAbdi added 3 commits August 13, 2021 23:19

Add support for virtual joints

0bbf978

Remove TransformListener from classes

1b7becd

clang-tidy

a9f96e0

JafarAbdi added 9 commits August 13, 2021 23:19

Update queue size

969be36

Fix typo

b1a2b0e

Fix rebase

fdc42d6

Address review

f383599

Fix tests

d3e0964

pre-commit

03319cb

Use C++17 features

29d766c

Add warning when user add tf2 buffer to a transform listener while us…

a92c03c

…ing csm

Revert change to save_to_warehouse since we don't start the state mon…

56f718c

…itor

JafarAbdi force-pushed the pr-virtual_joints branch from 9f3be3e to 56f718c Compare August 13, 2021 20:23

JafarAbdi and others added 2 commits August 13, 2021 23:24

Update moveit_ros/planning/planning_scene_monitor/src/current_state_m…

9055df0

…onitor.cpp Co-authored-by: Nathan Brooks <nbbrooks@gmail.com>

Update moveit_ros/planning/planning_scene_monitor/src/current_state_m…

ca29d1f

…onitor.cpp Co-authored-by: Nathan Brooks <nbbrooks@gmail.com>

henningkayser approved these changes Aug 16, 2021

View reviewed changes

tylerjw approved these changes Aug 17, 2021

View reviewed changes

tylerjw and others added 3 commits August 17, 2021 09:49

Merge branch 'main' into pr-virtual_joints

6d0d218

Merge branch 'main' into pr-virtual_joints

5bceb18

Merge branch 'main' into pr-virtual_joints

578664e

JafarAbdi mentioned this pull request Aug 17, 2021

Refactor PSM & CSM to have private tf_buffer & tf_listener #617

Closed

JafarAbdi merged commit 974a7a1 into moveit:main Aug 17, 2021

Galactic/Rolling - 2.3.0 - September 10 automation moved this from Review in progress to Done Aug 17, 2021

JafarAbdi deleted the pr-virtual_joints branch August 17, 2021 19:37

tylerjw pushed a commit to tylerjw/moveit2 that referenced this pull request Aug 27, 2021

Create a transform subscribers to enable virtual joints (moveit#310)

7ef6447

schornakj mentioned this pull request Aug 27, 2021

Planning Scene is missing TF frames #646

Closed

MikeWrock pushed a commit to MikeWrock/moveit2 that referenced this pull request Aug 15, 2022

Api Documentation CI (moveit#310)

f680cbe

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Create a transform subscribers to enable virtual joints #310

Create a transform subscribers to enable virtual joints #310

JafarAbdi commented Nov 18, 2020

codecov bot commented Nov 18, 2020 •

edited

henningkayser Nov 19, 2020

schornakj Apr 5, 2021

henningkayser Jun 10, 2021

JafarAbdi Aug 11, 2021 •

edited

henningkayser Nov 19, 2020

schornakj Apr 5, 2021

JafarAbdi Aug 11, 2021

henningkayser commented Nov 20, 2020

schornakj Apr 5, 2021

schornakj Apr 5, 2021

DLu commented May 4, 2021

henningkayser left a comment •

edited

henningkayser Jun 10, 2021

JafarAbdi Aug 11, 2021

henningkayser Jun 10, 2021

JafarAbdi commented Aug 11, 2021

nbbrooks Aug 13, 2021

henningkayser left a comment

tylerjw left a comment

	// allow update if time is more recent or if it is a static transform (time = 0)
	// skip if time is not more recent and is not a static transform (time = 0)

Create a transform subscribers to enable virtual joints #310

Create a transform subscribers to enable virtual joints #310

Conversation

JafarAbdi commented Nov 18, 2020

Description

Checklist

codecov bot commented Nov 18, 2020 • edited

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

JafarAbdi Aug 11, 2021 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

henningkayser commented Nov 20, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

DLu commented May 4, 2021

henningkayser left a comment • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

JafarAbdi commented Aug 11, 2021

Choose a reason for hiding this comment

henningkayser left a comment

Choose a reason for hiding this comment

tylerjw left a comment

Choose a reason for hiding this comment

codecov bot commented Nov 18, 2020 •

edited

JafarAbdi Aug 11, 2021 •

edited

henningkayser left a comment •

edited