[Ready] Port move_group to ROS2, fix some extra parameters #217

ddengster · 2020-06-16T06:01:16Z

Description

Siphoned from PR #187
~~Accompanies PR #204 if you want things to work beyond compiling.~~ merged!

Other changes include:

ompl_interface switched to plugin manager
parameters with '/' have been replaced by '.' in planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp, planning/planning_scene_monitor/src/planning_scene_monitor.cpp, planning/robot_model_loader/src/robot_model_loader.cpp
dynamic reconfigure for planning_scene_monitor and trajectory_execution_manager has been integrated into a callback. You can look at the old definitions in the respective .cfg files

move_group's test has not been ported due to dependencies, see here.

Edit: It's compiling, but should we delay the codecov to another PR?

Checklist

Required by CI: Code is auto formatted using clang-format

codecov · 2020-06-17T03:55:29Z

Codecov Report

Merging #217 into master will decrease coverage by 1.22%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##           master     #217      +/-   ##
==========================================
- Coverage   47.67%   46.45%   -1.23%     
==========================================
  Files         155      123      -32     
  Lines       15691    12781    -2910     
==========================================
- Hits         7481     5937    -1544     
+ Misses       8210     6844    -1366

Impacted Files	Coverage Δ
.../include/moveit/robot_model/revolute_joint_model.h	`0.00% <0.00%> (-100.00%)`	⬇️
...ry_processing/time_optimal_trajectory_generation.h	`0.00% <0.00%> (-100.00%)`	⬇️
...cessing/src/time_optimal_trajectory_generation.cpp	`0.00% <0.00%> (-74.59%)`	⬇️
...eit_core/collision_detection/src/collision_env.cpp	`54.90% <0.00%> (-18.31%)`	⬇️
...veit_core/robot_model/src/floating_joint_model.cpp	`25.54% <0.00%> (-9.79%)`	⬇️
moveit_core/planning_scene/src/planning_scene.cpp	`33.68% <0.00%> (-7.05%)`	⬇️
moveit_core/robot_state/src/robot_state.cpp	`29.79% <0.00%> (-4.44%)`	⬇️
...e/src/parameterization/model_based_state_space.cpp	`65.97% <0.00%> (-2.78%)`	⬇️
...lude/moveit/collision_detection/collision_common.h	`77.50% <0.00%> (-2.50%)`	⬇️
...bot_state/include/moveit/robot_state/robot_state.h	`85.89% <0.00%> (-1.93%)`	⬇️
... and 58 more

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 08b5fd9...6261057. Read the comment docs.

moveit_ros/move_group/CMakeLists.txt

henningkayser

This really looks almost done, just some minor comments to address.
With declared parameters the demo works out of the box for me (both on Foxy and Eloquent, rebased onto the latest branches). Also, thanks for fixing up the parameter callbacks, do you have a working example for this? The old cfg files can be removed.

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

moveit_ros/move_group/src/move_group_capability.cpp

henningkayser · 2020-08-05T13:54:26Z

moveit_ros/move_group/src/move_group.cpp

+      RCLCPP_INFO(LOGGER, "MoveGroup debug mode is OFF");
+
+    rclcpp::executors::MultiThreadedExecutor executor;
+    rclcpp::Node::SharedPtr monitor_node = rclcpp::Node::make_shared("monitor_node", opt);


Shouldn't the name be "move_group_node" or something like that? I think the PlanningSceneMonitor will use it's internal thread for monitoring anyway (#230)

this monitor_node is fed into planning_scene_monitor->startStateMonitor to prevent subscribers from never receiving messages. we can wait for #230 to go in first and adjust accordingly.

It would probably make sense to merge that one first and then adjust it here. Do you see any issues with the implementation in #230? Woudl be great to have a review that is based on an actual use case like this.

#230 is merged

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

moveit_ros/move_group/CMakeLists.txt

ddengster · 2020-08-18T07:53:10Z

Also, thanks for fixing up the parameter callbacks, do you have a working example for this?

I referenced from these sources:
https://index.ros.org/doc/ros2/Releases/Release-Foxy-Fitzroy/#deprecate-set-on-parameters-set-callback

https://github.com/ros2/rclcpp/blob/master/rclcpp_lifecycle/test/test_lifecycle_node.cpp#L421

edit: removed cfg file b1c82a0

henningkayser · 2020-08-18T12:45:12Z

@ddengster as this is about to be ready, could you squash the commit history into several chunks before merging? clang-format, clang-tidy fixes, cleanup, rebase etc clutter up the commit history but we really want to keep the meaningful steps in your development.

JafarAbdi · 2020-08-26T00:49:30Z

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

+  desc.set__name("publish_planning_scene");
+  desc.set__description("Set to True to publish Planning Scenes");
+  bool publish_planning_scene = node_->declare_parameter("planning_scene_monitor.publish_planning_scene", false, desc);
+
+  desc.set__name("publish_geometry_updates");
+  desc.set__description("Set to True to publish geometry updates of the planning scene");
+  bool publish_geom_updates = node_->declare_parameter("planning_scene_monitor.publish_geometry_updates", true, desc);
+
+  desc.set__name("publish_state_updates");
+  desc.set__description("Set to True to publish state updates of the planning scene");
+  bool publish_state_updates = node_->declare_parameter("planning_scene_monitor.publish_state_updates", false, desc);
+
+  desc.set__name("publish_transforms_updates");
+  desc.set__description("Set to True to publish transform updates of the planning scene");
+  bool publish_transform_updates = node_->declare_parameter("planning_scene_monitor.publish_transforms_updates", false, desc);
+
+  desc.set__type(rclcpp::ParameterType::PARAMETER_DOUBLE);
+  desc.set__name("publish_planning_scene_hz");
+  desc.set__description("Set the maximum frequency at which planning scene updates are published");
+  double publish_planning_scene_hz = node_->declare_parameter("planning_scene_monitor.publish_planning_scene_hz", 4.0, desc);


We have to check if the parameter is declared for the node before declaring it otherwise it will throw an exception, for example, if we create two monitors for the same node

has_parameter check before declare/get parameter 7f381b0

ddengster · 2020-08-26T10:19:35Z

@henningkayser i've cut it down to 28 commits, let me know if you want it squashed further

henningkayser

Looks good, there still seem to be some regressions I tagged with the comments. Commit history is a bit long but it seems very descriptive so I'm fine with it.

henningkayser · 2020-08-26T11:07:32Z

moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp

+    if (action_res->error_code.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED)
+    {
+      //@todo: wait for preempt?
+      // move_action_server_->setPreempted(action_res, response);


Shouldn't the goal just get canceled here?

iirc in ros1 there was setCanceled and setPreempted, i wasn't sure what the difference was. anyway, i changed this to canceled 44c1643

henningkayser · 2020-08-26T11:12:39Z

moveit_ros/move_group/src/move_group.cpp

@@ -38,6 +38,8 @@
 #include <tf2_ros/transform_listener.h>
 #include <moveit/move_group/capability_names.h>
 #include <moveit/move_group/move_group_capability.h>
+#include <moveit/move_group/move_group_context.h>


These two includes seem redundant. I guess they were removed with the sync but then just added again with the rebase.

removed redundant 44c1643

henningkayser · 2020-08-26T11:13:44Z

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -497,7 +528,7 @@ bool PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_
    throw std::runtime_error("requestPlanningSceneState() to self-provided service: " + service_name);
  }
  // use global namespace for service
-  auto client = pnode_->create_client<moveit_msgs::srv::GetPlanningScene>(service_name);
+  auto client = node_->create_client<moveit_msgs::srv::GetPlanningScene>(service_name);


Please revert, these and below should be pnode_

reverted 44c1643

JafarAbdi · 2020-08-26T12:50:10Z

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

-  // reconfigure_impl_ = new DynamicReconfigureImpl(this);
+
+  // Set up publishing parameters
+  rcl_interfaces::msg::ParameterDescriptor desc;


One of the lines below is causing a seg-fault, I wasn't able to determine which one but commenting them seems to fix it

Did you rebase onto #257? It could be the same timing issue

fixed parameter 44c1643

edit: i ran the demo, it seems to be working fine

henningkayser

@ddengster Looks good to me. Please consider merging https://github.com/ddengster/moveit2/pull/2 first, but then please rebase and lgtm

…able unneeded

fix current state monitor not listening for joint_state messages while waiting fix clock type exception

…ode, cleanup

fix move_group query_planners service name

…oblems

…gureImpl

…ameters

backport fixes, some cleanup clang format fixes more cleanup + clang format #2

clang format fixes #2 cmake style fixes, clang warning fixes revert clang warning fixes rebase fixes

… to controller_mgr_node clang format fixes #3 compile and clang tidy fix clang format fixes #4

remove unused variable warnings clang warning fixes fixed/aligned cmake fix cmake c++ standard namespace fix revert state time

…e_monitor

… fixes

remove redundant includes

ddengster mentioned this pull request Jun 16, 2020

[Help Wanted] Port Planning interface to ROS2 (except tests) #218

Closed

1 task

ddengster changed the title ~~[WIP] Port move_group to ROS2, fix some extra parameters~~ [Ready] Port move_group to ROS2, fix some extra parameters Jun 29, 2020

henningkayser reviewed Jul 23, 2020

View reviewed changes

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

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

ddengster requested a review from henningkayser August 7, 2020 03:59

henningkayser reviewed Aug 11, 2020

View reviewed changes

JafarAbdi approved these changes Aug 25, 2020

View reviewed changes

JafarAbdi mentioned this pull request Aug 26, 2020

[Draft/ReferenceOnly] Port motion_planning_rviz (with submodules robot_interaction, fake_controller_manager) #195

Closed

6 tasks

JafarAbdi reviewed Aug 26, 2020

View reviewed changes

henningkayser reviewed Aug 26, 2020

View reviewed changes

JafarAbdi reviewed Aug 26, 2020

View reviewed changes

ddengster requested a review from henningkayser August 27, 2020 04:03

henningkayser approved these changes Aug 27, 2020

View reviewed changes

ddengster added 14 commits August 28, 2020 11:52

Port move_group to ROS2

e3ecefa

port move_group capabilities to ROS2

9cef114

fix move_group main function, fix parameters, rdf initialization, dis…

2f9ade6

…able unneeded

fixed move_group main executor not spinning correctly

578e138

fix current state monitor not listening for joint_state messages while waiting fix clock type exception

rename action service

729bba2

replace spin_until_future_complete, fix deadlock with controllermgr n…

dfbee08

…ode, cleanup

fix move_group messages

f67324a

fix move_group query_planners service name

commented out unneeded loading of sensor manager which was causing pr…

1a52e3b

…oblems

port parameters from dyanmicconfig, commented out PSM::DynamicReconfi…

d22b64a

…gureImpl

fix parameters that were not in controller_mgr_node

bdf0090

added trajectory execution parameters

027b64d

fix kinematics, planning_pipeline, fix_start_state_bounds and PSM par…

e8567cc

…ameters

switch OMPL to use pluginlib

70f45bd

catersian path service compile fixes

0cbacef

ddengster and others added 16 commits August 28, 2020 11:52

compile fixes, fix cherry pick

980262c

backport fixes, some cleanup clang format fixes more cleanup + clang format #2

re-enable sensor_manager plugin loader

ae901be

fix and organize packages

fa74db2

clang format fixes #2 cmake style fixes, clang warning fixes revert clang warning fixes rebase fixes

port dynamic_reconfigure for planning_scene_monitor

b3ce4c0

remove dynamic reconfigure headers from PSM

22be1d6

cleanup, revert dynamic reconfigure uncomment, fix parameter transfer…

1658ff1

… to controller_mgr_node clang format fixes #3 compile and clang tidy fix clang format fixes #4

port dynamic parameters for trajectory_execution_manager

d9fb463

remove unused variable warnings clang warning fixes fixed/aligned cmake fix cmake c++ standard namespace fix revert state time

pushed node used for capabilities into MoveGroupContext

409bb91

switch PSM parameters to declare, add decription

d89d0af

add parameter publish_planning_scene_hz

756dce1

switch target_include_directories to PUBLIC

474de0b

removed PlanningSceneMonitorDynamicReconfigure.cfg from planning_scen…

1164dd5

…e_monitor

no longer using listening_node since PSM has its own executor, rebase…

ff29aa7

… fixes

get_parameter if it exists else declare

1a9f06a

fix preempt, revert wrong node, fix parameter not declared exception

4cf85b5

remove redundant includes

Refactor dynamic PSM parameters (#2)

6261057

henningkayser merged commit 273da39 into moveit:master Aug 28, 2020

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Ready] Port move_group to ROS2, fix some extra parameters #217

[Ready] Port move_group to ROS2, fix some extra parameters #217

ddengster commented Jun 16, 2020 •

edited

Loading

codecov bot commented Jun 17, 2020 •

edited

Loading

henningkayser left a comment

henningkayser Aug 5, 2020

ddengster Aug 18, 2020

henningkayser Aug 18, 2020

henningkayser Aug 18, 2020

ddengster commented Aug 18, 2020 •

edited

Loading

henningkayser commented Aug 18, 2020 •

edited

Loading

JafarAbdi Aug 26, 2020

ddengster Aug 26, 2020

ddengster commented Aug 26, 2020

henningkayser left a comment

henningkayser Aug 26, 2020

ddengster Aug 27, 2020

henningkayser Aug 26, 2020

ddengster Aug 27, 2020

henningkayser Aug 26, 2020

ddengster Aug 27, 2020

JafarAbdi Aug 26, 2020

henningkayser Aug 26, 2020

ddengster Aug 27, 2020 •

edited

Loading

henningkayser left a comment •

edited

Loading

[Ready] Port move_group to ROS2, fix some extra parameters #217

[Ready] Port move_group to ROS2, fix some extra parameters #217

Conversation

ddengster commented Jun 16, 2020 • edited Loading

Description

Checklist

codecov bot commented Jun 17, 2020 • edited Loading

Codecov Report

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ddengster commented Aug 18, 2020 • edited Loading

henningkayser commented Aug 18, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ddengster commented Aug 26, 2020

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

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

ddengster Aug 27, 2020 • edited Loading

Choose a reason for hiding this comment

henningkayser left a comment • edited Loading

Choose a reason for hiding this comment

ddengster commented Jun 16, 2020 •

edited

Loading

codecov bot commented Jun 17, 2020 •

edited

Loading

ddengster commented Aug 18, 2020 •

edited

Loading

henningkayser commented Aug 18, 2020 •

edited

Loading

ddengster Aug 27, 2020 •

edited

Loading

henningkayser left a comment •

edited

Loading