[MSA] ros2_control Integration #1299

DLu · 2022-06-01T14:12:05Z

Description

Modifies the existing ControllersWidget to generate moveit_controllers.yaml (Generating ros_controllers.yaml will be a different PR). The main functional difference is in the formatting of the yaml.

Edit: I realized I could reuse a lot of the existing functionality for both moveit_controllers.yaml and ros2_controllers.yaml.

Here is the full ros2_control flow:

URDF Modifications

ROS2 Controllers

MoveIt Controllers

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 · 2022-06-01T15:50:44Z

Codecov Report

Merging #1299 (fb88c6a) into feature/msa (5608368) will decrease coverage by 0.02%.
The diff coverage is n/a.

@@               Coverage Diff               @@
##           feature/msa    #1299      +/-   ##
===============================================
- Coverage        61.58%   61.56%   -0.01%     
===============================================
  Files              274      274              
  Lines            24966    24966              
===============================================
- Hits             15373    15369       -4     
- Misses            9593     9597       +4

Impacted Files	Coverage Δ
...nning_scene_monitor/src/planning_scene_monitor.cpp	`45.29% <0.00%> (-0.43%)`	⬇️

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 5608368...fb88c6a. Read the comment docs.

jliukkonen

I tested this locally and the result looks OK. I left couple questions about small things here and there.

One another thing I noticed while testing this is the following error:

I get that error after clicking the Auto Add FollowJointsTrajectory Controllers For Each Planning Group button. I get that error even when I do have planning groups defined (like in that screenshot). If the list of controllers is empty, no error, but clicking on the auto add button again shows that error popup. I didn't check if it is related to this particular PR but it might be fixable within this PR.

...tant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.hpp

jliukkonen · 2022-06-02T14:15:10Z

...tant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers_config.hpp

  /**
-   * \brief Adds a controller to ros_controllers_config_ vector
+   * \brief Adds a controller to controllers_config_ vector


It would be a nice improvement if every function followed this documentation style with brief, then parameters (input or output, hopefully never output or mixed) and return, when applicable.

moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_config.cpp

jliukkonen · 2022-06-02T14:36:36Z

moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_config.cpp

+    // TODO: Output possible trajectory_execution parameters including...
+    //       * allowed_execution_duration_scaling: 1.2
+    //       * allowed_goal_duration_margin: 0.5
+    //       * allowed_start_tolerance: 0.01


Consider adding these in this PR. 🙏

...p_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp

...tup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.hpp

vatanaksoytezer

I tested this and was able to generate ros2_control files without any problems. The launch files at least launch normally know. I'm still getting the following error when I'm launching demo.launch.py:

[spawner-5] [INFO] [1654786906.095245390] [spawner_panda_arm_hand_controller]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1654786906.095696045] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1654786908.102498881] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-5] [INFO] [1654786908.102495244] [spawner_panda_arm_hand_controller]: Waiting for '/controller_manager' services to be available
[spawner-5] [INFO] [1654786910.110568133] [spawner_panda_arm_hand_controller]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1654786910.110702876] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1654786912.118679627] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-5] [INFO] [1654786912.119128218] [spawner_panda_arm_hand_controller]: Waiting for '/controller_manager' services to be available
[spawner-6] [ERROR] [1654786914.127482870] [spawner_joint_state_broadcaster]: Controller manager not available
[spawner-5] [ERROR] [1654786914.128252414] [spawner_panda_arm_hand_controller]: Controller manager not available

which might be out of scope of this PR. Another TODO would be to fix the rviz configs as well., which is also out of scope of this PR. Thus, I'm happy to approve as is.

DLu · 2022-06-09T16:11:39Z

was able to generate ros2_control files without any problems. The launch files at least launch normally know. I'm still getting the following error when I'm launching demo.launch.py:

@vatanaksoytezer Was this with a clean panda config, or building off of the existing one?

Another TODO would be to fix the rviz configs as well., which is also out of scope of this PR.

@vatanaksoytezer Open a ticket? I'm not sure what you're talking about

vatanaksoytezer · 2022-06-09T16:15:50Z

@vatanaksoytezer Was this with a clean panda config, or building off of the existing one?

A fresh panda config I generated with this branch from scratch.

@vatanaksoytezer Open a ticket? I'm not sure what you're talking about

When we launch rviz, the motion planning, robot model, fixed frame etc. on rviz.

DLu · 2022-06-10T14:26:47Z

I'm still getting the following error when I'm launching demo.launch.py:

This should be fixed with the latest commit. panda.urdf.xacro was not being properly generated.

When we launch rviz, the motion planning, robot model, fixed frame etc. on rviz.

I resolved this with #1316, but its on main and not the feature branch. I recommend we resolve #1315 and #1332 before rebasing.

...tant/moveit_setup_controllers/include/moveit_setup_controllers/urdf_modifications_widget.hpp

AndyZe · 2022-06-13T20:14:15Z

moveit_setup_assistant/moveit_setup_controllers/src/moveit_controllers_config.cpp

+            emitter << YAML::Key << "action_ns" << YAML::Value << "follow_joint_trajectory";
+            emitter << YAML::Key << "default" << YAML::Value << "true";
+          }
+          else


I believe you could safely drop this, or make it an error. All of the MoveIt ControllerManager stuff only works with FollowJointTrajectory, so there would be no reason to configure anything else.

@JafarAbdi @schornakj do you agree?

Suggested change

else

else

{

// Throw error or something

RCLCPP_WARN_STREAM((*logger_),

"MoveIt controllers only work with FollowJointTrajectory-type controllers.");

return false;

}

moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp

AndyZe · 2022-06-13T20:20:12Z

moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp

+      if (controller.joints_.empty())
+      {


Is this basically creating one default joint, if none are defined? Prob should print a warning in this case.

Suggested change

if (controller.joints_.empty())

{

if (controller.joints_.empty())

{

RCLCPP_WARN_STREAM_ONCE((*logger_), "Did not find even one valid joint in the ros2_control yaml `joints` list. Creating a dummy default.");

...p_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp

moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro

...tup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.hpp

…/config/setup_step)

Co-authored-by: AndyZe <andyz@utexas.edu>

AndyZe · 2022-06-14T15:35:27Z

I'll keep an eye on this and merge when it passes CI

DLu · 2022-06-14T16:41:22Z

🎉

* Modified URDF Infrastructure * Control Xacro Config * URDF Modifications Widget * Use same base class for ROS2Controllers and MoveItControllers (widget/config/setup_step) * Further cleanup * Fix ModifiedURDF Generation * Apply suggestions from code review Co-authored-by: AndyZe <andyz@utexas.edu> * Rename button variable * Clang line length fix Co-authored-by: AndyZe <andyz@utexas.edu>

DLu requested a review from vatanaksoytezer June 1, 2022 14:12

vatanaksoytezer requested a review from JafarAbdi June 1, 2022 21:13

jliukkonen reviewed Jun 2, 2022

View reviewed changes

DLu force-pushed the feature/msa_moveit_control branch from c653b3c to d64892e Compare June 7, 2022 15:07

DLu changed the title ~~[MSA] Generate MoveIt Controllers~~ [MSA] ros2_control Integration Jun 7, 2022

DLu mentioned this pull request Jun 7, 2022

[MSA] Virtual Destructors #1324

Merged

6 tasks

DLu force-pushed the feature/msa_moveit_control branch from b34262f to e371b31 Compare June 8, 2022 17:01

DLu commented Jun 8, 2022

View reviewed changes

...p_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp Outdated Show resolved Hide resolved

DLu commented Jun 8, 2022

View reviewed changes

...p_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp Show resolved Hide resolved

DLu commented Jun 8, 2022

View reviewed changes

...tup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/ros2_controllers.hpp Outdated Show resolved Hide resolved

vatanaksoytezer approved these changes Jun 9, 2022

View reviewed changes

DLu requested review from jliukkonen, henningkayser and AndyZe June 13, 2022 14:06

moveit deleted a comment from mergify bot Jun 13, 2022

AndyZe reviewed Jun 13, 2022

View reviewed changes

AndyZe approved these changes Jun 13, 2022

View reviewed changes

vatanaksoytezer force-pushed the feature/msa branch from 91e6f17 to 5fabe99 Compare June 14, 2022 10:49

DLu and others added 8 commits June 14, 2022 10:24

Modified URDF Infrastructure

6fadec2

Control Xacro Config

ac995cc

URDF Modifications Widget

23c194c

Use same base class for ROS2Controllers and MoveItControllers (widget…

68b21b8

…/config/setup_step)

Further cleanup

180e987

Fix ModifiedURDF Generation

fc4308d

Apply suggestions from code review

1800265

Co-authored-by: AndyZe <andyz@utexas.edu>

Rename button variable

25551c3

DLu force-pushed the feature/msa_moveit_control branch from 2b4d321 to 25551c3 Compare June 14, 2022 14:55

Clang line length fix

fb88c6a

moveit deleted a comment from mergify bot Jun 14, 2022

AndyZe merged commit 087664b into moveit:feature/msa Jun 14, 2022

DLu deleted the feature/msa_moveit_control branch June 14, 2022 17:00

DLu mentioned this pull request Jun 14, 2022

[MSA] Port moveit_setup_controllers #1261

Closed

DLu mentioned this pull request Jun 16, 2022

Add option to automatically generate ros2_control's fake components description file #1283

Closed

6 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[MSA] ros2_control Integration #1299

[MSA] ros2_control Integration #1299

DLu commented Jun 1, 2022 •

edited

codecov bot commented Jun 1, 2022 •

edited

jliukkonen left a comment

jliukkonen Jun 2, 2022

jliukkonen Jun 2, 2022

vatanaksoytezer left a comment

DLu commented Jun 9, 2022

vatanaksoytezer commented Jun 9, 2022

DLu commented Jun 10, 2022 •

edited

AndyZe Jun 13, 2022

AndyZe Jun 13, 2022

AndyZe commented Jun 14, 2022

DLu commented Jun 14, 2022

-          else
+          else
+          {
+            // Throw error or something
+            RCLCPP_WARN_STREAM((*logger_),
+                           "MoveIt controllers only work with FollowJointTrajectory-type controllers.");
+            return false;
+          }

[MSA] ros2_control Integration #1299

[MSA] ros2_control Integration #1299

Conversation

DLu commented Jun 1, 2022 • edited

Description

URDF Modifications

ROS2 Controllers

MoveIt Controllers

Checklist

codecov bot commented Jun 1, 2022 • edited

Codecov Report

jliukkonen left a comment

Choose a reason for hiding this comment

jliukkonen Jun 2, 2022

Choose a reason for hiding this comment

jliukkonen Jun 2, 2022

Choose a reason for hiding this comment

vatanaksoytezer left a comment

Choose a reason for hiding this comment

DLu commented Jun 9, 2022

vatanaksoytezer commented Jun 9, 2022

DLu commented Jun 10, 2022 • edited

AndyZe Jun 13, 2022

Choose a reason for hiding this comment

AndyZe Jun 13, 2022

Choose a reason for hiding this comment

AndyZe commented Jun 14, 2022

DLu commented Jun 14, 2022

DLu commented Jun 1, 2022 •

edited

codecov bot commented Jun 1, 2022 •

edited

DLu commented Jun 10, 2022 •

edited