[Draft/ReferenceOnly] Port move_group node to ROS2 by ddengster · Pull Request #187 · moveit/moveit2

ddengster · 2020-05-13T06:28:40Z

This PR is done in conjunction with a 'coffeebot' gazebo simulation in which I use move_group to plan and execute trajectories. There will be some stuff missing that should be in the port, I'll try my best to highlight them. Feel free to siphon changes from this PR into the main branch as I will be trying to get the pipeline that decides how to get the poses into the srdf file working before I start merging changes from this PR.

There are other dependencies that have features that needed to be ported to ROS2. These are in the PRs:

joint_trajectory_controller, for updating the state of the robot.
gazebo_ros_control, for use with the gazebo simulator

Description

Port move_group node to ROS2. Submodules like capabilities have also been ported. Submodules regarding sensors have not been ported

Design problems regarding nodes:

Parameters and parameter file handling. Move_group used to have a number of classes that share parameters from a central ROS1 server. I've put them into the move_group node and that node flows throughout different classes in the codebase. The side effect is that you will get a number of deadlocks because that node is also used for subscribing and listening to the same topic. To fix this you'll have create or supply a different node. (see TrajectoryExecutionManager::initialize)
There's a good number of functions that uses rclcpp::spin_until_future_complete. It unfortunately gives me exceptions ('Node already added to executor') when I already have an existing executor to manage the execution of these nodes for me (see move_group.cpp main function). I recommend not using spin_until_future_complete.

To summarize, having separate nodes for each class has its pros and cons. This PR's style is having 1 node to contain all parameters and making new nodes and supplying them parameters as needed.

Modifications to launch and config files generated by moveit_setup_assistant:

There's a ton of xml launch files that I ported to a single python launch file just so things could be easier. In my experience the ROS2 xml launch repo isnt too stable for my liking, and xml is a data format ill-suited to the task that launching programs require. I recommend porting to python launch files.
config files are prepended with the following as listed in the ROS2 launch wiki:

<node name, usually 'move_group'>:
  ros__parameters:

ddengster · 2020-05-13T06:41:11Z

moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp

-        std::string value;
-        if (node_->get_parameter(group_name_param + "." + k, value))
+        rclcpp::Parameter param = node_->get_parameter(group_name_param + "." + k);
+        if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING)


same fix mentioned in issue #184

ddengster · 2020-05-13T06:42:04Z

moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp

+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  ompl_interface::OMPLPlannerManager, planning_interface::PlannerManager)


made OMPLPlannerManager switch to plugin system

ddengster · 2020-05-13T06:42:49Z

...controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h

      auto cancel_result_future = controller_action_client_->async_cancel_goal(current_goal_);
+      //@todo: replace rclcpp::spin_until_future_complete
      if (rclcpp::spin_until_future_complete(node_, cancel_result_future) !=
          rclcpp::executor::FutureReturnCode::SUCCESS)


replace rclcpp::spin_until_future_complete

So, for std::shared_future results from async calls we should just use wait() wait_for() or wait_until() since the executor is already doing the spinning.

ddengster · 2020-05-13T06:44:04Z

moveit_ros/move_group/include/moveit/move_group/capability_names.h

 static const std::string SET_PLANNER_PARAMS_SERVICE_NAME =
    "set_planner_params";                                 // service name to set planner parameters
-static const std::string MOVE_ACTION = "move_group";      // name of 'move' action
+static const std::string MOVE_ACTION = "move_action";      // name of 'move' action


name conflict. Suggest replacement to be "move_action" instead of "move_group"

Interesting, conflict with what exactly? In general I don't see anything wrong with renaming this, though.

ddengster · 2020-05-13T06:46:57Z

moveit_ros/move_group/src/move_group.cpp

+  rclcpp::NodeOptions opt;
+  opt.allow_undeclared_parameters(true);
+  opt.automatically_declare_parameters_from_overrides(true);
+  rclcpp::Node::SharedPtr nh = rclcpp::Node::make_shared("move_group", opt);


This is the main function, and our main node is "move_group". We supply it with parameters and pass this node throughout the codebase.

ddengster · 2020-05-13T06:50:32Z

moveit_ros/move_group/src/move_group.cpp

+    std::string robot_desc_param = "robot_description";
+    std::string str = getParameterFromRemoteNode<std::string>(nh, "robot_state_publisher", robot_desc_param);
+    nh->declare_parameter(robot_desc_param);
+    nh->set_parameter(rclcpp::Parameter(robot_desc_param, str));


The robot_description parameter is fetched from robot_state_publisher. Not sure if #169 will help fix it? Also I've had to reuse this code to supply the robot_description parameter to my other codebases.

to clarify 'my other codebases' meaning the codebases that use MoveGroupInterface or PlanningSceneInterface

ddengster · 2020-05-13T06:51:17Z

moveit_ros/move_group/src/move_group.cpp

+    RCLCPP_INFO(LOGGER, "%s", semantic_file.c_str());
+    std::stringstream buffer;
+    buffer << file.rdbuf();
+    nh->set_parameter(rclcpp::Parameter("robot_description_semantic", buffer.str()));


Similar to the above, I've had to reuse this code to supply the robot_description_semantic parameter to my other codebases. Not sure if there's a better way of doing it.

ddengster · 2020-05-13T06:56:07Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

+        for (auto param : allparams)
+        {
+          //RCLCPP_INFO(LOGGER, "%s", param.first.c_str());
+          controller_mgr_node_->set_parameter(rclcpp::Parameter(param.first, param.second));


for a quick hack we copy parameters from move_group to the controller_mgr node. I'd imagine something more specific with code standards.

ddengster · 2020-05-13T06:57:05Z

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

  // only report that execution finished successfully when the robot actually stopped moving
  if (last_execution_status_ == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
-    waitForRobotToStop(*trajectories_[i - 1]);
+    waitForRobotToStop(*trajectories_[i - 1], 10);


increasing wait times from default 1 to 10. Not share if we can have a parameter instead?

ddengster · 2020-05-13T06:59:32Z

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

+    //@todo: switch out for callbacks
+    int64_t timeout = 3;
+    if (rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(timeout)) !=
+      rclcpp::executor::FutureReturnCode::SUCCESS)


should not use rclcpp::spin_until_future_complete

ddengster · 2020-05-13T07:00:03Z

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

+    return res.result->error_code;
  }

  MoveItErrorCode execute(const Plan& plan, bool wait)


function of major interest

ddengster · 2020-05-13T07:00:11Z

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

+    return pick(constructPickupGoal(object.id, std::move(response->grasps), plan_only));
  }

  MoveItErrorCode plan(Plan& plan)


function of major interest

henningkayser · 2020-05-13T14:45:20Z

@ddengster first of all, great work and thanks a lot for your effort! We were actually about to spin-up development again to make progress towards the full release, so I'm really appreciating your contributions.

This PR is done in conjunction with a 'coffeebot' gazebo simulation in which I use move_group to plan and execute trajectories. There will be some stuff missing that should be in the port, I'll try my best to highlight them. Feel free to siphon changes from this PR into the main branch as I will be trying to get the pipeline that decides how to get the poses into the srdf file working before I start merging changes from this PR.

In general, we do fresh package/library migrations piece by piece so full pipeline functionality is not required out of the box.

There are other dependencies that have features that needed to be ported to ROS2. These are in the PRs:

joint_trajectory_controller, for updating the state of the robot.

gazebo_ros_control, for use with the gazebo simulator

Nice!

Description

Port move_group node to ROS2. Submodules like capabilities have also been ported. Submodules regarding sensors have not been ported

Design problems regarding nodes:

Parameters and parameter file handling. Move_group used to have a number of classes that share parameters from a central ROS1 server. I've put them into the move_group node and that node flows throughout different classes in the codebase. The side effect is that you will get a number of deadlocks because that node is also used for subscribing and listening to the same topic. To fix this you'll have create or supply a different node. (see TrajectoryExecutionManager::initialize)

Yes, with MoveGroup we're now at a point where we have to make some non-trivial design decisions. I see two basic approaches: 1. Implement a "global" parameter server that resembles the ROS1 pattern; 2. Refactor parameter propagation between classes and enable parameter syncing between nodes that require it. We're about to investigate these and related changes the next weeks.

There's a good number of functions that uses rclcpp::spin_until_future_complete. It unfortunately gives me exceptions ('Node already added to executor') when I already have an existing executor to manage the execution of these nodes for me (see move_group.cpp main function). I recommend not using spin_until_future_complete.

I fully agree, this is a pain point for which we still need to discuss some best practices.

To summarize, having separate nodes for each class has its pros and cons. This PR's style is having 1 node to contain all parameters and making new nodes and supplying them parameters as needed.

Absolutely the right approach for now. Our goal is to first have a full straight port which can be synced with MoveIt 1 and only then we start making further ROS2 related refactorings.

Modifications to launch and config files generated by moveit_setup_assistant:

There's a ton of xml launch files that I ported to a single python launch file just so things could be easier. In my experience the ROS2 xml launch repo isnt too stable for my liking, and xml is a data format ill-suited to the task that launching programs require. I recommend porting to python launch files.

config files are prepended with the following as listed in the ROS2 launch wiki:
<node name, usually 'move_group'>:
  ros__parameters:

Using python launch files is the way to go IMO. That means that we might have to redesign some parts of the MSA which should also include considerations about a potential multi-node setup or how we want to design parameter lookup.

Right now I'm working on a development roadmap and scheduling weekly sync meetings to get the project kicked-off. Since your contributions appear like you are working on this more long-term, are you interested in joining the meetings to get introduced and collaborate on some of the points discussed here?

davetcoleman · 2020-05-13T15:19:06Z

Great effort @ddengster, thanks for sharing this and we look forward to getting it merged!

ddengster · 2020-05-14T09:39:55Z

Since your contributions appear like you are working on this more long-term, are you interested in joining the meetings to get introduced and collaborate on some of the points discussed here?

Yea I'd like to know who's working on what, and to give you a heads-up as to what I'm working on so as to reduce conflicts. Thing is I'm on GMT+8 Singapore time and might not be able to attend US timings. Do you have a google doc for the meetings so I can at least write down what I'm working on atm or read what's happening?

henningkayser · 2020-05-14T11:57:40Z

Since your contributions appear like you are working on this more long-term, are you interested in joining the meetings to get introduced and collaborate on some of the points discussed here?

Yea I'd like to know who's working on what, and to give you a heads-up as to what I'm working on so as to reduce conflicts. Thing is I'm on GMT+8 Singapore time and might not be able to attend US timings. Do you have a google doc for the meetings so I can at least write down what I'm working on atm or read what's happening?

We don't have the meeting schedule fixed yet, but it will share plans and results publicly here on Github. I'm at GMT+2 myself, but maybe we can find a time that works for all. Could you send me a mail so that we can sync up?

henningkayser

Looks pretty solid overall, great work! I'll give this a couple runs the next days.

henningkayser · 2020-05-14T12:07:56Z

...controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h

      auto cancel_result_future = controller_action_client_->async_cancel_goal(current_goal_);
+      //@todo: replace rclcpp::spin_until_future_complete
      if (rclcpp::spin_until_future_complete(node_, cancel_result_future) !=
          rclcpp::executor::FutureReturnCode::SUCCESS)


So, for std::shared_future results from async calls we should just use wait() wait_for() or wait_until() since the executor is already doing the spinning.

henningkayser · 2020-05-14T12:11:32Z

...controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h

-              rclcpp::executor::FutureReturnCode::SUCCESS);
+
+    rclcpp::Time start_time = node_->now();
+    auto end_time = start_time + timeout;


Same here. Also, the default timeout = rclcpp::Duration(0) should indicate an infinite timeout

henningkayser · 2020-05-14T12:16:36Z

...t_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp


  if (done_)
-    RCLCPP_DEBUG_STREAM(LOGGER, "sending trajectory to " << name_);
+    RCLCPP_INFO_STREAM(LOGGER, "sending trajectory to " << name_);


Why change the log level?

henningkayser · 2020-05-14T12:17:58Z

...t_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp

  // Send goal
  auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options);
-  if (rclcpp::spin_until_future_complete(node_, current_goal_future) != rclcpp::executor::FutureReturnCode::SUCCESS)
+  std::future_status status = current_goal_future.wait_for(std::chrono::seconds(60));


Shouldn't this timeout be the expected execution time + some margin?

henningkayser · 2020-05-14T12:18:49Z

moveit_ros/manipulation/move_group_pick_place_capability/CMakeLists.txt

 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

-target_link_libraries(${MOVEIT_LIB_NAME} moveit_pick_place_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(${MOVEIT_LIB_NAME} moveit_pick_place_planner ${LIBS} ${Boost_LIBRARIES})


Is Boost still required?

henningkayser · 2020-05-14T12:19:52Z

moveit_ros/move_group/CMakeLists.txt

+  PROPERTIES
+  VERSION "${${PROJECT_NAME}_VERSION}")
+
+# Causes the visibility macros to use dllexport rather than dllimport,


I assume you're working on Windows? ;)

henningkayser · 2020-05-14T12:21:12Z

moveit_ros/move_group/CMakeLists.txt

-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+install(TARGETS moveit_move_group_capabilities_base
+  moveit_move_group_default_capabilities
+  ARCHIVE


Just a nit, but I think it reads a little better with the old format

henningkayser · 2020-05-14T12:24:52Z

moveit_ros/move_group/include/moveit/move_group/capability_names.h

 static const std::string SET_PLANNER_PARAMS_SERVICE_NAME =
    "set_planner_params";                                 // service name to set planner parameters
-static const std::string MOVE_ACTION = "move_group";      // name of 'move' action
+static const std::string MOVE_ACTION = "move_action";      // name of 'move' action


Interesting, conflict with what exactly? In general I don't see anything wrong with renaming this, though.

henningkayser · 2020-05-14T12:29:45Z

moveit_ros/move_group/include/moveit/move_group/move_group_capability.h

  {
  }

+  void setNode(const rclcpp::Node::SharedPtr& node);


This and setContext() are only called once before initialize(), right? I would pass node and context as parameters of initialize() and remove the setters (similar to how the planning adapters are initialized now).

henningkayser · 2020-05-14T12:31:05Z

moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp

 namespace move_group
 {
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_move_group_default_capabilities.apply_planning_scene_service_capability");


I see, it's really super long. I would probably just remove the "default_capabilities" in the middle, actually.

ddengster · 2020-05-15T03:42:30Z

Could you send me a mail so that we can sync up?

I've sent the email

…oblems

… execute

…e waiting

…ode, cleanup

…unported action clients

ddengster · 2020-08-31T07:35:00Z

closing since move_group is merged

ddengster commented May 13, 2020

View reviewed changes

This was referenced May 13, 2020

[Reference Only/Draft] joint_trajectory_controller convert trajectories to effort values ros-controls/ros2_controllers#36

Closed

[Reference Only/Draft] gazebo_ros_control ROS2 port ros-simulation/gazebo_ros_pkgs#1083

Closed

ddengster changed the title ~~Draft/ReferenceOnly Port move_group node to ROS2~~ [Draft/ReferenceOnly] Port move_group node to ROS2 May 13, 2020

ddengster commented May 13, 2020

View reviewed changes

henningkayser reviewed May 14, 2020

View reviewed changes

ddengster mentioned this pull request May 20, 2020

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

Closed

6 tasks

ddengster added 17 commits June 1, 2020 11:12

Port move_group to ROS2

88146b4

include fix

99dbe7f

port planning_interface to ROS2

328b2d7

port move_group capabilities to ROS2

6380c02

port move_group to ros2, fix parameters

37bf3e3

port kinematics, planner and request adapter plugins to ROS2

93d8fd7

more ompl parameters fixes, removed todo

95a0a54

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

9c76c35

…oblems

fixed executor not spinning correctly

890a8bb

fix ompl not parsing parameters correctly, and some debug prints

b3b743f

fix logging and executor-node collision issues for functions plan and…

ce29453

… execute

port parameters from dyanmicconfig

f951b83

fix current state monitor not listening for joint_state messages whil…

ea0e767

…e waiting

fix clock type issue

a641994

rename action service

cf05213

removed comment

92b375b

plan and execute logging text change

85234d8

ddengster added 8 commits June 1, 2020 11:12

replace spin_until_future_complete, fix deadlock with controllermgr n…

13d8bc5

…ode, cleanup

fix time comparison exception

9e296ab

fix parameters that were not in controller_mgr_node

06c9f22

added trajectory execution parameters, added wait time for robot stop

90285c1

fix move_group messages

660b8c2

fix move_group query_planners service name

e461ffc

move_group_interface replace spin_until_future_complete and disabled …

b8fc3ac

…unported action clients

fix duration warnings

7ae9e98

davetcoleman force-pushed the master branch from bc870ad to 759281f Compare June 2, 2020 00:33

move_group_interface fix move(), cleanup plan() and execute()

a0bf034

ddengster mentioned this pull request Jun 2, 2020

[Ready] simple_controller_manager fixes from PR #187, remove spin_until_future_complete #204

Merged

1 task

davetcoleman force-pushed the master branch from 759281f to a558de9 Compare June 3, 2020 17:21

This was referenced Jun 12, 2020

Port Robot interaction to ROS2 #211

Merged

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

Merged

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

Closed

ddengster closed this Aug 31, 2020

Conversation

ddengster commented May 13, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

henningkayser commented May 13, 2020

Description

Uh oh!

davetcoleman commented May 13, 2020

Uh oh!

ddengster commented May 14, 2020

Uh oh!

henningkayser commented May 14, 2020

Uh oh!

henningkayser left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ddengster commented May 15, 2020

Uh oh!

ddengster commented Aug 31, 2020

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

ddengster commented May 13, 2020 •

edited

Loading