Merge branch 'rolling' into fix/compilation_instructions/rolling

.github/workflows/ci-rolling.yaml

@@ -3,9 +3,9 @@ name: gz_ros2_control CI - Rolling
 on:
   workflow_dispatch:
   pull_request:
-    branches: [ master ]
+    branches: [ rolling ]
   push:
-    branches: [ master ]
+    branches: [ rolling ]
   schedule:
     # Run every morning to detect flakiness and broken dependencies
     - cron: '03 5 * * *'

doc/index.rst

@@ -139,9 +139,22 @@ robot hardware interfaces between *ros2_control* and Gazebo.
 The *gz_ros2_control* ``<plugin>`` tag also has the following optional child elements:
 
 * ``<parameters>``: A YAML file with the configuration of the controllers. This element can be given multiple times to load multiple files.
-* ``<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.
 * ``<controller_manager_name>``: Set controller manager name (default: ``controller_manager``)
 
+The following additional parameters can be set via child elements in the URDF or via ROS parameters in the YAML file above:
+
+* ``<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.
+* ``<position_proportional_gain>``: Set the proportional gain. (default: 0.1) This determines the setpoint for a position-controlled joint ``joint_velocity = joint_position_error * position_proportional_gain``.
+
+or via ROS parameters:
+
+.. code-block:: yaml
+
+  gz_ros_control:
+    ros__parameters:
+      hold_joints: false
+      position_proportional_gain: 0.5
+
 Additionally, one can specify a namespace and remapping rules, which will be forwarded to the controller_manager and loaded controllers. Add the following ``<ros>`` section:
 
 .. code-block:: xml

gz_ros2_control/src/gz_ros2_control_plugin.cpp

@@ -315,6 +315,11 @@ void GazeboSimROS2ControlPlugin::Configure(
     hold_joints =
       sdfPtr->GetElement("hold_joints")->Get<bool>();
   }
+  double position_proportional_gain = 0.1;  // default
+  if (sdfPtr->HasElement("position_proportional_gain")) {
+    position_proportional_gain =
+      sdfPtr->GetElement("position_proportional_gain")->Get<double>();
+  }
 
   if (sdfPtr->HasElement("ros")) {
     sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
@@ -400,6 +405,32 @@ void GazeboSimROS2ControlPlugin::Configure(
       e.what());
   }
 
+  try {
+    this->dataPtr->node_->declare_parameter(
+      "position_proportional_gain",
+      rclcpp::ParameterValue(position_proportional_gain));
+  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+      "Parameter 'position_proportional_gain' has already been declared, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParametersException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+      "Parameter 'position_proportional_gain' has invalid name, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+      "Parameter 'position_proportional_gain' value is invalid, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+      "Parameter 'position_proportional_gain' value has wrong type, %s",
+      e.what());
+  }
+
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
     std::make_unique<gz_ros2_control::GZResourceManager>(this->dataPtr->node_, _ecm, enabledJoints);
 

gz_ros2_control/src/gz_system.cpp

@@ -200,14 +200,30 @@ bool GazeboSimSystem::initSim(
 
   this->dataPtr->joints_.resize(this->dataPtr->n_dof_);
 
-  constexpr double default_gain = 0.1;
-
   try {
-    this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter<double>(
-      "position_proportional_gain", default_gain);
-  } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) {
-    this->nh_->get_parameter(
-      "position_proportional_gain", this->dataPtr->position_proportional_gain_);
+    this->dataPtr->position_proportional_gain_ =
+      this->nh_->get_parameter("position_proportional_gain").as_double();
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'position_proportional_gain' not initialized, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(),
+      "Using default value: " << this->dataPtr->position_proportional_gain_);
+  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'position_proportional_gain' not declared, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(),
+      "Using default value: " << this->dataPtr->position_proportional_gain_);
+  } catch (rclcpp::ParameterTypeException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'position_proportional_gain' has wrong type: %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(),
+      "Using default value: " << this->dataPtr->position_proportional_gain_);
   }
 
   RCLCPP_INFO_STREAM(

gz_ros2_control_demos/config/ackermann_drive_controller.yaml

@@ -5,12 +5,9 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-
-    ackermann_steering_controller:
-      type: 'ackermann_steering_controller/AckermannSteeringController'
-
 ackermann_steering_controller:
   ros__parameters:
+    type: 'ackermann_steering_controller/AckermannSteeringController'
     wheelbase: 1.7
     front_wheel_track: 1.0
     rear_wheel_track: 1.0

gz_ros2_control_demos/config/cart_controller_effort.yaml

@@ -2,13 +2,11 @@ controller_manager:
   ros__parameters:
     update_rate: 1000  # Hz
 
-    effort_controller:
-      type: effort_controllers/JointGroupEffortController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 effort_controller:
   ros__parameters:
+    type: effort_controllers/JointGroupEffortController
     joints:
       - slider_to_cart

gz_ros2_control_demos/config/cart_controller_position.yaml

@@ -2,14 +2,12 @@ controller_manager:
   ros__parameters:
     update_rate: 1000  # Hz
 
-    joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 joint_trajectory_controller:
   ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
     joints:
       - slider_to_cart
     command_interfaces:

gz_ros2_control_demos/config/cart_controller_velocity.yaml

@@ -2,21 +2,17 @@ controller_manager:
   ros__parameters:
     update_rate: 1000  # Hz
 
-    velocity_controller:
-      type: velocity_controllers/JointGroupVelocityController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    imu_sensor_broadcaster:
-      type: imu_sensor_broadcaster/IMUSensorBroadcaster
-
 velocity_controller:
   ros__parameters:
+    type: velocity_controllers/JointGroupVelocityController
     joints:
       - slider_to_cart
 
 imu_sensor_broadcaster:
   ros__parameters:
+    type: imu_sensor_broadcaster/IMUSensorBroadcaster
     sensor_name: cart_imu_sensor
     frame_id: imu

gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml

@@ -5,11 +5,9 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    diff_drive_base_controller:
-      type: diff_drive_controller/DiffDriveController
-
 diff_drive_base_controller:
   ros__parameters:
+    type: diff_drive_controller/DiffDriveController
     left_wheel_names: ["left_wheel_joint"]
     right_wheel_names: ["right_wheel_joint"]
 

gz_ros2_control_demos/config/gripper_controller_effort.yaml

@@ -2,14 +2,12 @@ controller_manager:
   ros__parameters:
     update_rate: 100  # Hz
 
-    gripper_controller:
-      type: forward_command_controller/ForwardCommandController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 gripper_controller:
   ros__parameters:
+    type: forward_command_controller/ForwardCommandController
     joints:
       - right_finger_joint
     interface_name: effort

gz_ros2_control_demos/config/gripper_controller_position.yaml

@@ -2,14 +2,12 @@ controller_manager:
   ros__parameters:
     update_rate: 100  # Hz
 
-    gripper_controller:
-      type: forward_command_controller/ForwardCommandController
-
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 gripper_controller:
   ros__parameters:
+    type: forward_command_controller/ForwardCommandController
     joints:
       - right_finger_joint
     interface_name: position

gz_ros2_control_demos/config/tricycle_drive_controller.yaml

@@ -2,18 +2,15 @@ controller_manager:
   ros__parameters:
     update_rate: 50 # Hz
 
-    tricycle_controller:
-      type: tricycle_controller/TricycleController
-
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
 joint_state_broadcaster:
   ros__parameters:
+    type: joint_state_broadcaster/JointStateBroadcaster
     extra_joints: ["right_wheel_joint", "left_wheel_joint"]
 
 tricycle_controller:
   ros__parameters:
+    type: tricycle_controller/TricycleController
+
     # Model
     traction_joint_name: traction_joint # Name of traction joint in URDF
     steering_joint_name: steering_joint # Name of steering joint in URDF

gz_ros2_control_demos/examples/example_ackermann_drive.cpp

@@ -17,7 +17,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 using namespace std::chrono_literals;
 
@@ -28,22 +28,26 @@ int main(int argc, char * argv[])
   std::shared_ptr<rclcpp::Node> node =
     std::make_shared<rclcpp::Node>("ackermann_drive_test_node");
 
-  auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
-    "/ackermann_steering_controller/reference_unstamped", 10);
+  auto publisher = node->create_publisher<geometry_msgs::msg::TwistStamped>(
+    "/ackermann_steering_controller/reference", 10);
 
   RCLCPP_INFO(node->get_logger(), "node created");
 
-  geometry_msgs::msg::Twist command;
+  geometry_msgs::msg::Twist tw;
 
-  command.linear.x = 0.5;
-  command.linear.y = 0.0;
-  command.linear.z = 0.0;
+  tw.linear.x = 0.5;
+  tw.linear.y = 0.0;
+  tw.linear.z = 0.0;
 
-  command.angular.x = 0.0;
-  command.angular.y = 0.0;
-  command.angular.z = 0.3;
+  tw.angular.x = 0.0;
+  tw.angular.y = 0.0;
+  tw.angular.z = 0.3;
+
+  geometry_msgs::msg::TwistStamped command;
+  command.twist = tw;
 
   while (1) {
+    command.header.stamp = node->now();
     publisher->publish(command);
     std::this_thread::sleep_for(50ms);
     rclcpp::spin_some(node);

gz_ros2_control_demos/examples/example_diff_drive.cpp

@@ -34,8 +34,6 @@ int main(int argc, char * argv[])
 
   geometry_msgs::msg::TwistStamped command;
 
-  command.header.stamp = node->now();
-
   command.twist.linear.x = 0.1;
   command.twist.linear.y = 0.0;
   command.twist.linear.z = 0.0;
@@ -45,6 +43,7 @@ int main(int argc, char * argv[])
   command.twist.angular.z = 0.1;
 
   while (1) {
+    command.header.stamp = node->now();
     publisher->publish(command);
     std::this_thread::sleep_for(50ms);
     rclcpp::spin_some(node);

gz_ros2_control_demos/examples/example_gripper.cpp

@@ -23,13 +23,11 @@
 
 #include "std_msgs/msg/float64_multi_array.hpp"
 
-std::shared_ptr<rclcpp::Node> node;
-
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
 
-  node = std::make_shared<rclcpp::Node>("gripper_test_node");
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("gripper_test_node");
 
   auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
     "/gripper_controller/commands", 10);

gz_ros2_control_demos/examples/example_position.cpp

@@ -154,7 +154,7 @@ int main(int argc, char * argv[])
     node.reset();
     return 1;
   }
-  RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
+  RCLCPP_INFO(node->get_logger(), "send goal call ok :)");
 
   rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
     goal_handle = goal_handle_future.get();
@@ -164,7 +164,7 @@ int main(int argc, char * argv[])
     node.reset();
     return 1;
   }
-  RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
+  RCLCPP_INFO(node->get_logger(), "Goal was accepted by server");
 
   // Wait for the server to be done with the goal
   auto result_future = action_client->async_get_result(goal_handle);

gz_ros2_control_demos/examples/example_tricycle_drive.cpp

@@ -34,8 +34,6 @@ int main(int argc, char * argv[])
 
   geometry_msgs::msg::TwistStamped command;
 
-  command.header.stamp = node->now();
-
   command.twist.linear.x = 0.2;
   command.twist.linear.y = 0.0;
   command.twist.linear.z = 0.0;
@@ -45,6 +43,7 @@ int main(int argc, char * argv[])
   command.twist.angular.z = 0.1;
 
   while (1) {
+    command.header.stamp = node->now();
     publisher->publish(command);
     std::this_thread::sleep_for(50ms);
     rclcpp::spin_some(node);