Merge branch 'master' into ahcorde/rolling/create_with_arguments

doc/index.rst

@@ -146,13 +146,14 @@ robot hardware interfaces between *ros2_control* and Gazebo.
       <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
         <robot_param>robot_description</robot_param>
         <robot_param_node>robot_state_publisher</robot_param_node>
-        <parameters>$(find gz_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
+        <parameters>$(find gz_ros2_control_demos)/config/cart_controller.yaml</parameters>
       </plugin>
   </gazebo>
 
 The *gz_ros2_control* ``<plugin>`` tag also has the following optional child elements:
 
 * ``<parameters>``: YAML file with the configuration of the controllers
+* ``<hold_joints>``: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet.
 
 Default gz_ros2_control Behavior
 -----------------------------------------------------------
@@ -200,32 +201,18 @@ and use the tag ``<controller_manager_prefix_node_name>`` to set the controller
 
   <gazebo>
     <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
-      <parameters>$(find gz_ros2_control_demos)/config/cartpole_controller.yaml</parameters>
+      <parameters>$(find gz_ros2_control_demos)/config/cart_controller.yaml</parameters>
       <controller_manager_prefix_node_name>controller_manager</controller_manager_prefix_node_name>
     </plugin>
   <gazebo>
 
-This controller publishes the state of all resources registered to a
-``hardware_interface::StateInterface`` to a topic of type ``sensor_msgs/msg/JointState``.
-The following is a basic configuration of the controller.
+The following is a basic configuration of the controllers:
 
-.. code-block:: yaml
+- ``joint_state_broadcaster``: This controller publishes the state of all resources registered to a ``hardware_interface::StateInterface`` to a topic of type ``sensor_msgs/msg/JointState``.
+- ``joint_trajectory_controller``: This controller creates an action called ``/joint_trajectory_controller/follow_joint_trajectory`` of type ``control_msgs::action::FollowJointTrajectory``.
 
-  joint_state_controller:
-    ros__parameters:
-      type: joint_state_controller/JointStateController
-
-This controller creates an action called ``/cart_pole_controller/follow_joint_trajectory`` of type ``control_msgs::action::FollowJointTrajectory``.
-
-.. code-block:: yaml
-
-  cart_pole_controller:
-    ros__parameters:
-      type: joint_trajectory_controller/JointTrajectoryController
-      joints:
-        - slider_to_cart
-      write_op_modes:
-        - slider_to_cart
+.. literalinclude:: ../gz_ros2_control_demos/config/cart_controller_position.yaml
+   :language: yaml
 
 
 gz_ros2_control_demos

gz_ros2_control/src/gz_ros2_control_plugin.cpp

@@ -297,8 +297,20 @@ void GazeboSimROS2ControlPlugin::Configure(
 
   // Get controller manager node name
   std::string controllerManagerNodeName{"controller_manager"};
+
+  if (sdfPtr->HasElement("controller_manager_name")) {
+    controllerManagerNodeName = sdfPtr->GetElement("controller_manager_name")->Get<std::string>();
+  }
+
   std::string ns = "/";
 
+  // Hold joints if no control mode is active?
+  bool hold_joints = true;  // default
+  if (sdfPtr->HasElement("hold_joints")) {
+    hold_joints =
+      sdfPtr->GetElement("hold_joints")->Get<bool>();
+  }
+
   if (sdfPtr->HasElement("ros")) {
     sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
 
@@ -417,6 +429,27 @@ void GazeboSimROS2ControlPlugin::Configure(
         ex.what());
       continue;
     }
+
+    try {
+      this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
+    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParametersException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
+        e.what());
+    }
+
     if (!gzSimSystem->initSim(
         this->dataPtr->node_,
         enabledJoints,

gz_ros2_control/src/gz_system.cpp

@@ -175,6 +175,9 @@ class gz_ros2_control::GazeboSimSystemPrivate
 
   /// \brief Gain which converts position error to a velocity command
   double position_proportional_gain_;
+
+  // Should hold the joints if no control_mode is active
+  bool hold_joints_ = true;
 };
 
 namespace gz_ros2_control
@@ -196,6 +199,31 @@ bool GazeboSimSystem::initSim(
 
   this->dataPtr->update_rate = &update_rate;
 
+  try {
+    this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool();
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not initialized, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not declared, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::ParameterTypeException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' has wrong type: %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  }
+  RCLCPP_DEBUG_STREAM(
+    this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl);
+
+
   RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %lu", this->dataPtr->n_dof_);
 
   this->dataPtr->joints_.resize(this->dataPtr->n_dof_);
@@ -658,7 +686,7 @@ hardware_interface::return_type GazeboSimSystem::write(
         *jointEffortCmd = sim::components::JointForceCmd(
           {this->dataPtr->joints_[i].joint_effort_cmd});
       }
-    } else if (this->dataPtr->joints_[i].is_actuated) {
+    } else if (this->dataPtr->joints_[i].is_actuated && this->dataPtr->hold_joints_) {
       // Fallback case is a velocity command of zero (only for actuated joints)
       double target_vel = 0.0;
       auto vel =

gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml

@@ -32,7 +32,7 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
+    use_stamped_vel: true
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits

gz_ros2_control_demos/config/tricycle_drive_controller.yaml

@@ -50,7 +50,7 @@ tricycle_controller:
 
     # cmd_vel input
     cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received
-    use_stamped_vel: false # Set to True if using TwistStamped.
+    use_stamped_vel: true # Set to True if using TwistStamped.
 
     # Debug
     publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.

gz_ros2_control_demos/examples/example_diff_drive.cpp

@@ -16,7 +16,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 using namespace std::chrono_literals;
 
@@ -27,20 +27,22 @@ int main(int argc, char * argv[])
   std::shared_ptr<rclcpp::Node> node =
     std::make_shared<rclcpp::Node>("diff_drive_test_node");
 
-  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
-    "/diff_drive_base_controller/cmd_vel_unstamped", 10);
+  auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
+    "/diff_drive_base_controller/cmd_vel", 10);
 
   RCLCPP_INFO(node->get_logger(), "node created");
 
-  geometry_msgs::msg::Twist command;
+  geometry_msgs::msg::TwistStamped command;
 
-  command.linear.x = 0.1;
-  command.linear.y = 0.0;
-  command.linear.z = 0.0;
+  command.header.stamp = node->now();
 
-  command.angular.x = 0.1;
-  command.angular.y = 0.0;
-  command.angular.z = 0.0;
+  command.twist.linear.x = 0.1;
+  command.twist.linear.y = 0.0;
+  command.twist.linear.z = 0.0;
+
+  command.twist.angular.x = 0.0;
+  command.twist.angular.y = 0.0;
+  command.twist.angular.z = 0.1;
 
   while (1) {
     publisher->publish(command);

gz_ros2_control_demos/examples/example_tricycle_drive.cpp

@@ -16,7 +16,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 using namespace std::chrono_literals;
 
@@ -27,20 +27,22 @@ int main(int argc, char * argv[])
   std::shared_ptr<rclcpp::Node> node =
     std::make_shared<rclcpp::Node>("tricycle_drive_test_node");
 
-  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
+  auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
     "/tricycle_controller/cmd_vel", 10);
 
   RCLCPP_INFO(node->get_logger(), "node created");
 
-  geometry_msgs::msg::Twist command;
+  geometry_msgs::msg::TwistStamped command;
 
-  command.linear.x = 0.2;
-  command.linear.y = 0.0;
-  command.linear.z = 0.0;
+  command.header.stamp = node->now();
 
-  command.angular.x = 0.0;
-  command.angular.y = 0.0;
-  command.angular.z = 0.1;
+  command.twist.linear.x = 0.2;
+  command.twist.linear.y = 0.0;
+  command.twist.linear.z = 0.0;
+
+  command.twist.angular.x = 0.0;
+  command.twist.angular.y = 0.0;
+  command.twist.angular.z = 0.1;
 
   while (1) {
     publisher->publish(command);

gz_ros2_control_demos/launch/cart_example_effort.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
         executable='create',
         output='screen',
         parameters=[{'string': doc.toxml(),
-                     'name': 'cartpole',
+                     'name': 'cart',
                      'allow_renaming': True}],
     )
 

gz_ros2_control_demos/launch/cart_example_position.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
         executable='create',
         output='screen',
         parameters=[{'string': doc.toxml(),
-                     'name': 'cartpole',
+                     'name': 'cart',
                      'allow_renaming': True}],
     )
 

gz_ros2_control_demos/launch/cart_example_velocity.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
         executable='create',
         output='screen',
         parameters=[{'string': doc.toxml(),
-                     'name': 'cartpole',
+                     'name': 'cart',
                      'allow_renaming': True}],
     )
 

gz_ros2_control_demos/launch/diff_drive_example.launch.py

@@ -57,7 +57,7 @@ def generate_launch_description():
         executable='create',
         output='screen',
         parameters=[{'string': doc.toxml(),
-                     'name': 'cartpole',
+                     'name': 'diff_drive',
                      'allow_renaming': True}],
     )
 

gz_ros2_control_demos/launch/tricycle_drive_example.launch.py

@@ -57,7 +57,7 @@ def generate_launch_description():
         executable='create',
         output='screen',
         parameters=[{'string': doc.toxml(),
-                     'name': 'cartpole',
+                     'name': 'tricyle',
                      'allow_renaming': True}],
     )
 

gz_ros2_control_demos/urdf/test_cart_effort.xacro.urdf

@@ -1,8 +1,4 @@
 <?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from cartpole.xacro.urdf            | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
 <robot name="cartopole">
   <link name="world"/>
   <link name="slideBar">
@@ -83,7 +79,7 @@
   <gazebo>
     <!-- Joint state publisher -->
     <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-      <parameters>$(find gz_ros2_control_demos)/config/cartpole_controller_effort.yaml</parameters>
+      <parameters>$(find gz_ros2_control_demos)/config/cart_controller_effort.yaml</parameters>
     </plugin>
   </gazebo>
 </robot>

gz_ros2_control_demos/urdf/test_cart_position.xacro.urdf

@@ -1,8 +1,4 @@
 <?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from cartpole.xacro.urdf            | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
 <robot name="cartopole">
   <link name="world"/>
   <link name="slideBar">
@@ -95,7 +91,7 @@
   <gazebo>
     <!-- Joint state publisher -->
     <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-      <parameters>$(find gz_ros2_control_demos)/config/cartpole_controller_position.yaml</parameters>
+      <parameters>$(find gz_ros2_control_demos)/config/cart_controller_position.yaml</parameters>
     </plugin>
   </gazebo>
 </robot>

gz_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf

@@ -1,8 +1,4 @@
 <?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from cartpole.xacro.urdf            | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
 <robot name="cartopole">
   <link name="world"/>
   <link name="slideBar">
@@ -166,7 +162,7 @@
   <gazebo>
     <!-- Joint state publisher -->
     <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-      <parameters>$(find gz_ros2_control_demos)/config/cartpole_controller_velocity.yaml</parameters>
+      <parameters>$(find gz_ros2_control_demos)/config/cart_controller_velocity.yaml</parameters>
     </plugin>
     <plugin
       filename="ignition-sim-imu-system"

gz_ros2_control_tests/tests/position_test.py

@@ -77,7 +77,7 @@ def generate_test_description():
         executable='create',
         output='screen',
         arguments=['-string', doc.toxml(),
-                   '-name', 'cartpole',
+                   '-name', 'cart',
                    '-allow_renaming', 'true'],
     )
 

gz_ros2_control_tests/urdf/test_cart_position.xacro.urdf

@@ -1,8 +1,4 @@
 <?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from cartpole.xacro.urdf            | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
 <robot name="cartopole">
   <link name="world"/>
   <link name="slideBar">
@@ -95,7 +91,7 @@
   <gazebo>
     <!-- Joint state publisher -->
     <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-      <parameters>$(find gz_ros2_control_tests)/config/cartpole_controller_position.yaml</parameters>
+      <parameters>$(find gz_ros2_control_tests)/config/cart_controller_position.yaml</parameters>
     </plugin>
   </gazebo>
 </robot>