[Servo] Port moveit_servo to ROS2 #248

AdamPettinger · 2020-08-17T16:04:42Z

Description

This is a huge PR for porting moveit_servo to ROS2:

Ports the source
Adds examples (C++ interface, composable node interface, teleoperation demo for gamepad)
Adds integration and unit tests

Currently relies on moveit_resources, but I understand that might change soon. When it does, I will update and push here to use the new version

henningkayser

Just a very quick first round. Great work @AdamPettinger this looks very high quality with all the documentation, tests, examples. I'll give this a more in-depth review and testing the next days. Your launch files look like we could get some best practices out of it.

moveit_ros/moveit_servo/CMakeLists.txt

henningkayser · 2020-08-17T16:13:36Z

moveit_ros/moveit_servo/CMakeLists.txt


-find_package(catkin REQUIRED COMPONENTS
+set(LIBRARY_NAME moveit_servo)


I like the idea of using variables like this. Alternatively, we could follow the ROS 1 convention is to use patterns like ${PROJECT_NAME}_lib, ${PROJECT_NAME}_server.

I don't mind either way. There are other variables set here which don't fit nicely into ${PROJECT_NAME}_lib or ${PROJECT_NAME}_server

moveit_ros/moveit_servo/CMakeLists.txt

moveit_ros/moveit_servo/include/moveit_servo/collision_check.h

moveit_ros/moveit_servo/include/moveit_servo/servo.h

moveit_ros/moveit_servo/src/collision_check.cpp

AdamPettinger · 2020-08-17T17:14:49Z

Your launch files look like we could get some best practices out of it.

Agree @henningkayser. I definitely want to make those changes. I am just not sure what exactly those best practices are (or if they have even decided?) 😃

AndyZe

Looks really good. Just some nitpicks and questions

moveit_ros/moveit_servo/CMakeLists.txt

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

moveit_ros/moveit_servo/doc/running_the_demos.md

moveit_ros/moveit_servo/doc/servo_tutorial.md

moveit_ros/moveit_servo/src/collision_check.cpp

moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp

moveit_ros/moveit_servo/test/test_low_pass_filter.cpp

moveit_ros/moveit_servo/test/test_parameter_struct.hpp

moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.cpp

moveit_ros/moveit_servo/include/moveit_servo/servo_server.h

AndyZe

Overall I'm shocked at how thorough the tests are. Good job! I'll probably come back to nitpick a bit more, then test it

moveit_ros/moveit_servo/test/servo_launch_test_common.hpp

AndyZe · 2020-08-18T14:58:48Z

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -674,75 +688,80 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co

 void ServoCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta)


let's definitely make sure the velocity limit enforcement gets updated, like we discussed. Here's the ROS1 PR:

moveit/moveit#2260

moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.cpp

henningkayser · 2020-08-19T08:52:15Z

moveit_ros/moveit_servo/test/unit_test_servo_calcs.cpp

+{
+  // "Load" parameters from yaml as node parameters
+  std::string robot_description_string, srdf_string;
+  loadModelFile("panda_description/urdf/panda.urdf", robot_description_string);


Better use the convenience functions from moveit_test_utils to load your robot model.

Servo is looking for the URDF and SRDF as parameters (strings). To set that for testing I was doing:

loadModelFile("panda_description/urdf/panda.urdf", robot_description_string); loadModelFile("panda_moveit_config/config/panda.srdf", srdf_string); node_->declare_parameter<std::string>("robot_description", robot_description_string); node_->declare_parameter<std::string>("robot_description_semantic", srdf_string);

I don't see a way in the moveit_test_utils to give the path to the file and get back the string for loading as a parameter, but maybe I am missing something?

If you only want to use a moveit_resources package moveit_test_utils is the way to go. If you want to support any robot using the robot_description parameter, the PSM (or the RobotModelLoader) supports this without anything extra and the moveit_resources default should be passed with the launch file. If your actual use case is not covered, maybe it should go into moveit_test_utils.

I see, moveit_resources doesn't really help because the PSM expects either the robot_description parameter or a RobotModelLoader (not the RobotModel). Maybe it would still be a good idea to move a convenience function for this to test utils. Something that declares the robot description parameters from a default package and optionally provides you with the loader instance.

moveit_ros/moveit_servo/CMakeLists.txt

moveit_ros/moveit_servo/src/cpp_interface_demo/servo_cpp_interface_demo.cpp

henningkayser · 2020-08-20T13:18:21Z

moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.cpp

+  bool got_param = false;
+  try
+  {
+    node->declare_parameter<T>(param_name, T{});


I don't really understand why this should be necessary, but maybe I just didn't get the API right. I thought declare_parameter() already returns the correctly typed parameter value (default value or override from yaml) where trivial casts have already been applied (i.e. if you declare a double, an int should be casted to one). If value conversion is not successful, declare_parameter() will throw an InvalidParameterValueException. After declaring a parameter, get_parameter() will always return true (at least that's what I thought) so we would actually have to verify that the output value doesn't match the possibly invalid default value. The false case would only check for the rclcpp::ParameterType::PARAMETER_NOT_SET flag which is only used if undeclared parameters are allowed.

I think you are right, I will look into this. In general I am unsure how to tell if a parameter was successfully set from yaml vs just taken as the default value. There are some parameters in servo that can work as the default, but the user might not want that. And a bunch of course that cannot be default, and should be caught here as invalid

OK, I did some reworking on the parameter reading. You were correct about the get_parameter() always returning true after the declare_parameter() call. From the documentation, it does seem like the type casting with declare_parameter() would work, but I couldn't get it to and was still struggling with some doubles being read as ints (which could be fixed by writing 1.0 instead of 1 in the yaml, but that is definitely confusing for a user). Example of what didn't work, and still threw the error the catch looks for here:
parameters->low_pass_filter_coeff = node->declare_parameter<double>(ns + ".low_pass_filter_coeff", -1);

The change I just made turns that template function (declareAndGetParam()) into a void return and basically just uses it to catch the double-int problem.

davetcoleman

Incredible test coverage, great work!

davetcoleman · 2020-08-20T14:50:29Z

moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h

@@ -34,7 +34,7 @@
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*******************************************************************************/
+ *******************************************************************************/



Possible out of scope for this PR, but algorithm-heavy files like this would be better to live in moveit_core and only the ROS bindings are in moveit_ros. Would make these components much more reusable

I am ok with moving some of these out, in this PR or a followup one

davetcoleman · 2020-08-20T14:51:42Z

moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h

@@ -34,7 +34,7 @@
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*******************************************************************************/
+ *******************************************************************************/


This is a generic utility and would much better live in this folder for all of moveit to use:

https://github.com/ros-planning/moveit/tree/master/moveit_core/macros/include/moveit/macros

davetcoleman · 2020-08-20T14:52:23Z

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h

@@ -34,41 +34,46 @@
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*******************************************************************************/
+ *******************************************************************************/



Can this file also live in moveit_core?

davetcoleman · 2020-08-20T14:53:04Z

moveit_ros/moveit_servo/launch/servo_cpp_interface_demo.launch.py

@@ -0,0 +1,80 @@
+import os


Would this be better to live in moveit_tutorials?

I think so, but I don't see anywhere for moveit2_tutorials yet

AdamPettinger · 2020-08-20T20:32:10Z

Force pushed to rebase my work on recent master changes, but did not go all the way to the master branch tip as I am experiencing 2 issues:

Duplicate nodes, see Duplicate nodes in run_moveit_cpp demo #252
Seg faults while destructing the unit test fixture that cause the tests to fail (I think this might be related to the above issue as well: the planning_scene_monitor destructor is where the faults happen)

AndyZe

Just tested all of the demos. My only complaint is that CPU load is higher than it should be (the ROS1 version was at ~20%, the ROS2 version is about 200%). But, that might be a ROS2 middleware issue and not even your fault. Well done!

tylerjw

Reading through this it looks really good. Thank you for doing this! 🥇

tylerjw · 2020-08-21T22:16:42Z

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

  private_executor_->cancel();
+  if (private_executor_thread_.joinable())
+    private_executor_thread_.join();


Note for others, this fixed a segfault. The issue was that cancel just asks the executor to exit, it doesn't wait for it to happen which means that the memory was being freed while it was still being used.

tylerjw · 2020-08-21T22:37:35Z

Unused parameter warnings and clang-format is why it is failing CI.

I played with the demo using an xbox controller and it is really awesome.

tylerjw · 2020-08-22T20:58:41Z

It looks like the clang-tidy CI failures are mostly just naming convention issues: https://travis-ci.com/github/ros-planning/moveit2/jobs/376436677

I'm looking into the test failures to see if I can understand them.

tylerjw · 2020-08-23T07:34:47Z

Created a small PR for some slight adjustments to tests to make them run faster and pass: AdamPettinger#2

tylerjw · 2020-08-23T19:02:41Z

I'm happy to merge this but would like this rebased into a single (or two if you want my few changes separate) commits. The primary reason is to keep the commit history on master nice and make it easier when we go to release and create changelists. Here is my suggestion for the commit message (please change this to include anything you think would be important to include in the changelist):

Port of moveit_servo

* Ports the source from MoveIt
* Adds examples (C++ interface, composable node interface, teleoperation demo for gamepad)
* Adds integration and unit tests

The easiest way to do this would be to do an interactive rebase and squash most of the commits into one. The changes you made after mine can probably be applied in a different order (squashed into your changes) if you want to make this in two commits.

* Ports the source from MoveIt * Adds examples (C++ interface, composable node interface, teleoperation demo for gamepad) * Adds integration and unit tests

henningkayser reviewed Aug 17, 2020

View reviewed changes

AdamPettinger force-pushed the pr-rebase_on_foxy branch from c2d4961 to 6ad7833 Compare August 17, 2020 16:40

AndyZe reviewed Aug 18, 2020

View reviewed changes

moveit_ros/moveit_servo/test/servo_launch_test_common.hpp Show resolved Hide resolved

moveit_ros/moveit_servo/test/servo_launch_test_common.hpp Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/test/servo_launch_test_common.hpp Outdated Show resolved Hide resolved

AndyZe reviewed Aug 18, 2020

View reviewed changes

tylerjw reviewed Aug 18, 2020

View reviewed changes

henningkayser reviewed Aug 19, 2020

View reviewed changes

AndyZe reviewed Aug 19, 2020

View reviewed changes

moveit_ros/moveit_servo/CMakeLists.txt Outdated Show resolved Hide resolved

AndyZe reviewed Aug 19, 2020

View reviewed changes

moveit_ros/moveit_servo/src/cpp_interface_demo/servo_cpp_interface_demo.cpp Outdated Show resolved Hide resolved

henningkayser reviewed Aug 20, 2020

View reviewed changes

davetcoleman reviewed Aug 20, 2020

View reviewed changes

AdamPettinger force-pushed the pr-rebase_on_foxy branch from 8f3109b to 8a35218 Compare August 20, 2020 20:12

AdamPettinger force-pushed the pr-rebase_on_foxy branch from 8a35218 to 04eb050 Compare August 21, 2020 20:04

AndyZe approved these changes Aug 21, 2020

View reviewed changes

tylerjw approved these changes Aug 21, 2020

View reviewed changes

AdamPettinger and others added 2 commits August 23, 2020 20:16

Port of moveit_servo

db1f322

* Ports the source from MoveIt * Adds examples (C++ interface, composable node interface, teleoperation demo for gamepad) * Adds integration and unit tests

reduce test time and fix clang-tidy

0dbb33c

AdamPettinger force-pushed the pr-rebase_on_foxy branch from aa7bf6d to 0dbb33c Compare August 24, 2020 01:21

tylerjw merged commit b85ba87 into moveit:master Aug 24, 2020

henningkayser added a commit that referenced this pull request Aug 24, 2020

Port moveit_servo to ROS2 (#248)

e8c34a8

AdamPettinger mentioned this pull request Aug 26, 2020

GSoC: Jog Arm ROS2 Port #206

Closed

AdamPettinger deleted the pr-rebase_on_foxy branch June 2, 2021 19:58

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Servo] Port moveit_servo to ROS2 #248

[Servo] Port moveit_servo to ROS2 #248

AdamPettinger commented Aug 17, 2020

henningkayser left a comment

henningkayser Aug 17, 2020

AdamPettinger Aug 18, 2020

AdamPettinger commented Aug 17, 2020

AndyZe left a comment

AndyZe left a comment

AndyZe Aug 18, 2020

henningkayser Aug 19, 2020 •

edited

Loading

AdamPettinger Aug 19, 2020

henningkayser Aug 19, 2020

henningkayser Aug 19, 2020

henningkayser Aug 20, 2020

AdamPettinger Aug 20, 2020

AdamPettinger Aug 22, 2020

davetcoleman left a comment

davetcoleman Aug 20, 2020

AdamPettinger Aug 21, 2020

davetcoleman Aug 20, 2020

davetcoleman Aug 20, 2020

davetcoleman Aug 20, 2020

AdamPettinger Aug 21, 2020

AdamPettinger commented Aug 20, 2020

AndyZe left a comment

tylerjw left a comment

tylerjw Aug 21, 2020

tylerjw commented Aug 21, 2020 •

edited

Loading

tylerjw commented Aug 22, 2020

tylerjw commented Aug 23, 2020

tylerjw commented Aug 23, 2020


		find_package(catkin REQUIRED COMPONENTS
		set(LIBRARY_NAME moveit_servo)

		@@ -674,75 +688,80 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co

		void ServoCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta)

[Servo] Port moveit_servo to ROS2 #248

[Servo] Port moveit_servo to ROS2 #248

Conversation

AdamPettinger commented Aug 17, 2020

Description

henningkayser left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

AdamPettinger commented Aug 17, 2020

AndyZe left a comment

Choose a reason for hiding this comment

AndyZe left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

henningkayser Aug 19, 2020 • edited Loading

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman left a comment

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

AdamPettinger commented Aug 20, 2020

AndyZe left a comment

Choose a reason for hiding this comment

tylerjw left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tylerjw commented Aug 21, 2020 • edited Loading

tylerjw commented Aug 22, 2020

tylerjw commented Aug 23, 2020

tylerjw commented Aug 23, 2020

henningkayser Aug 19, 2020 •

edited

Loading

tylerjw commented Aug 21, 2020 •

edited

Loading