[Reference Only/Draft] gazebo_ros_control ROS2 port #1083

ddengster · 2020-04-22T07:26:55Z

The code changes in this PR was intended to work with a 'coffeebot' simulation in gazebo. It has lots of hacks, and is only meant to be an incomplete guide to porting to ROS2. Used in conjunction with ros-controls/ros2_controllers#36

Consider commits after the 18th Feb 2020, or e474da1.

Things of note will be put in reviews.

TLDR notes for porters:

Update: Also used with moveit2 training moveit/moveit2#187

Signed-off-by: Louise Poubel <louise@openrobotics.org>

All other sensor publishers were updated previously to use the same profile (#926). I'm not sure if the image publishers were overlooked or the image_transport API didn't support setting the QoS profile at the time. Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* [ros2] make transient local reliable (#1033) * make transient local reliable Signed-off-by: Karsten Knese <karsten@openrobotics.org> * fix master Signed-off-by: Karsten Knese <karsten@openrobotics.org> * add launch test Signed-off-by: Karsten Knese <karsten@openrobotics.org> * make it actual latched Signed-off-by: Karsten Knese <karsten@openrobotics.org> * alpha sort Signed-off-by: Karsten Knese <karsten@openrobotics.org> * add launch_test dependency Signed-off-by: Karsten Knese <karsten@openrobotics.org> * more dependencies Signed-off-by: Karsten Knese <karsten@openrobotics.org> * remove debug print Signed-off-by: Karsten Knese <karsten@openrobotics.org> * is_initialized -> ok Signed-off-by: Karsten Knese <karsten@openrobotics.org> * Update gazebo_ros/test/entity_spawner.test.py Co-Authored-By: chapulina <louise@openrobotics.org> * use erase-remove idiom Signed-off-by: Karsten Knese <karsten@openrobotics.org> * use ReadyToTest() Signed-off-by: Karsten Knese <karsten@openrobotics.org> Co-authored-by: chapulina <louise@openrobotics.org> * Set timeout and call gzserver directly Signed-off-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: chapulina <louise@openrobotics.org>

ddengster · 2020-04-22T07:32:28Z

gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h

+  hardware_interface::JointStateInterface    js_interface_;
+  hardware_interface::EffortJointInterface   ej_interface_;
+  hardware_interface::PositionJointInterface pj_interface_;
+  hardware_interface::VelocityJointInterface vj_interface_;


my coffeebot simulation uses a function in EffortJointInterface to translate trajectories from joint_trajectory_controller to effort values. Hence the rest is disabled.

ddengster · 2020-04-22T07:33:18Z

gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h

+  joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
+  joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
+  joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
+  joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;


Similar to the above, I only use a function from EffortJointSaturationInterface to clamp my effort values.

ddengster · 2020-04-22T07:36:57Z

gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h

+  std::vector<double> joint_vel_limits_;
+  std::vector<ControlMethod> joint_control_methods_;
+#if 0
+  std::vector<control_toolbox::Pid> pid_controllers_;


I havent found use for control_toolbox::Pid in gazebo_ros_controls, hence I disabled it. However, in another joint_trajectory_controller draft I used control_toolbox::Pid as part the code for converting trajectories to effort values. I'll link the joint_trajectory_controller PR later.

ros-controls/ros2_controllers#36

ddengster · 2020-04-22T07:38:31Z

gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h

+  // Controller manager
+  std::shared_ptr<controller_manager::ControllerManager> controller_manager_;
+  std::shared_ptr<controller_interface::ControllerInterface> controller_;
+  std::shared_ptr<controller_interface::ControllerInterface> controller2_;


These 2 controllers are custom code. Normally you'd get the controller_manager to spawn controllers but this functionality is not available as of this writing. Might be wise to wait for that functionality before porting.

ddengster · 2020-04-22T07:40:06Z

gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h

+  /// \brief Gazebo plugin version of RobotHW
+  ///
+  /// An object of class RobotHWSim represents a robot's simulated hardware.
+  class RobotHWSim : public hardware_interface::RobotHardware


RobotHWSim used to inherit from hardware_interface::RobotHW, but in ros2 it has changed to hardware_interface::RobotHardware (and has different functionality to account for)

ddengster · 2020-04-22T07:40:39Z

gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h

+
+    //@note: Avoid using these functions! ros2_control was not built with gazebo_ros_control 
+    //in mind, keep to using readSim and writeSim so we don't have to update the documents for now
+    virtual hardware_interface::hardware_interface_ret_t init() { }


as per comment, also check above.

ddengster · 2020-04-22T07:42:11Z

gazebo_ros_control/src/default_robot_hw_sim.cpp

+#else
+  for (auto eff_limit_handle : joint_eff_limit_handles_)
+  {
+    eff_limit_handle.enforceLimits(period);


limits handles applied here.

ddengster · 2020-04-22T07:49:13Z

gazebo_ros_control/src/default_robot_hw_sim.cpp

+
+      //@note (ddeng): these joint limits arent input into the node, so fetch them from the urdf
+      limits.min_position = urdf_joint->limits->lower;
+      limits.max_position = urdf_joint->limits->upper;


This function DefaultRobotHWSim::registerJointLimits needs some trimming; it has a bunch of repeat logic.

ddengster · 2020-04-22T07:52:22Z

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp

+
+    load_params_from_yaml(controller_->get_lifecycle_node(), param_file, "coffeebot_arm_effort_controller");
+    if (controller2_)
+      load_params_from_yaml(controller2_->get_lifecycle_node(), param_file, "coffeebot_gripper_effort_controller");


Custom code: I load 2 controllers and associated parameter files here. Again, this functionality should be replaced by controller_manager spawn controller functionality when it has been implemented.

ddengster · 2020-04-22T07:55:15Z

gazebo_ros_control/src/gazebo_ros_control_plugin.cpp

+
+    controller_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("period", sim_period.seconds()));
+    if (controller2_)
+      controller2_->get_lifecycle_node()->set_parameter(rclcpp::Parameter("period", sim_period.seconds()));


In ROS1, iirc each controller derives off Gazebo::Node, which supplies the "period" parameter automatically. In ROS2, each controller derives off LifecycleNode and the functionality interface with Gazebo::Node hasn't been done yet. Porters should take this into account.

ddengster · 2020-04-22T08:17:37Z

gazebo_ros_control/src/default_robot_hw_sim.cpp

+    joint_eff_cmdhandle_[j] = hardware_interface::JointCommandHandle(
+      joint_names_[j], &joint_effort_command_[j]);
+    //should be register_joint_effort_command_handle, but there's only 1 buffer for now
+    if (register_joint_command_handle(&joint_eff_cmdhandle_[j]) != hardware_interface::HW_RET_OK) {


The joint commands are registered here. The controllers (joint_trajectory_controller in this case) then use the registered commands. Porters should account for how these values are fed into the controllers.

chapulina · 2020-06-09T21:11:42Z

Closing this in favor of https://github.com/ros-simulation/gazebo_ros2_control

j-rivero and others added 15 commits October 3, 2019 20:39

Update changelogs

772ac49

3.4.0

bbe4cfe

generate a .dsv file for the environment hook

99dc702

Update changelogs

bbd9ced

3.4.1

8ce85ea

Merge branch 'ros2' into eloquent

d72c320

changelog

88e4990

Signed-off-by: Louise Poubel <louise@openrobotics.org>

3.4.2

9836c73

Add initial ros_control files for porting

e474da1

coffeebot plugin now working for certain trajectories

4dd5c21

cleanup and remove errors

61679f6

More cleanup

7ecebd8

fix c++ version

cdaf9ea

ddengster commented Apr 22, 2020

View reviewed changes

ddengster mentioned this pull request Apr 22, 2020

Move gazebo_ros_control into separate git repository? #179

Closed

ddengster commented Apr 22, 2020

View reviewed changes

ddengster mentioned this pull request Apr 22, 2020

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

Closed

ddengster closed this Apr 22, 2020

ddengster reopened this Apr 22, 2020

chapulina added the ros2 label Apr 22, 2020

added joint state controller to publish joint states to moveit2

d1e9116

ddengster mentioned this pull request May 13, 2020

[Draft/ReferenceOnly] Port move_group node to ROS2 moveit/moveit2#187

Closed

chapulina marked this pull request as draft May 20, 2020 18:34

ahcorde mentioned this pull request May 28, 2020

Added initial version of gazebo_ros2_control ros-controls/gazebo_ros2_control#1

Merged

chapulina closed this Jun 9, 2020

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[Reference Only/Draft] gazebo_ros_control ROS2 port #1083

[Reference Only/Draft] gazebo_ros_control ROS2 port #1083

ddengster commented Apr 22, 2020 •

edited

Loading

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020 •

edited

Loading

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020

ddengster Apr 22, 2020

chapulina commented Jun 9, 2020

[Reference Only/Draft] gazebo_ros_control ROS2 port #1083

[Reference Only/Draft] gazebo_ros_control ROS2 port #1083

Conversation

ddengster commented Apr 22, 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

ddengster Apr 22, 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

Choose a reason for hiding this comment

chapulina commented Jun 9, 2020

ddengster commented Apr 22, 2020 •

edited

Loading

ddengster Apr 22, 2020 •

edited

Loading