demo_diff_drive_controller

ros2_control_demo_hardware/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(rclcpp REQUIRED)
 add_library(
   ${PROJECT_NAME}
   SHARED
+  src/diffbot_system.cpp
   src/rrbot_system_position_only.cpp
   src/rrbot_system_multi_interface.cpp
 )

ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp

@@ -0,0 +1,72 @@
+// Copyright 2021 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_
+#define ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "hardware_interface/base_interface.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/hardware_interface_status_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "ros2_control_demo_hardware/visibility_control.h"
+
+namespace ros2_control_demo_hardware
+{
+class DiffBotSystemHardware
+: public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware);
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type start() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type stop() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type read() override;
+
+  ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
+  hardware_interface::return_type write() override;
+
+private:
+  // Parameters for the DiffBot simulation
+  double hw_start_sec_;
+  double hw_stop_sec_;
+
+  // Store the command for the simulated robot
+  std::vector<double> hw_commands_;
+  std::vector<double> hw_states_;
+};
+
+}  // namespace ros2_control_demo_hardware
+
+#endif  // ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_

ros2_control_demo_hardware/ros2_control_demo_hardware.xml

@@ -13,4 +13,11 @@
       The ROS2 Control minimal robot protocol for a system with multiple command and state interfaces.
     </description>
   </class>
+  <class name="ros2_control_demo_hardware/DiffBotSystemHardware"
+         type="ros2_control_demo_hardware::DiffBotSystemHardware"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      The ros2_control DiffBot example using a system hardware interface-type. It uses velocity command and position state interface. The example is the starting point to implement a hardware interface for differential-drive mobile robots.
+    </description>
+  </class>
 </library>

ros2_control_demo_hardware/src/diffbot_system.cpp

@@ -0,0 +1,209 @@
+// Copyright 2021 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ros2_control_demo_hardware/diffbot_system.hpp"
+
+#include <chrono>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace ros2_control_demo_hardware
+{
+hardware_interface::return_type DiffBotSystemHardware::configure(
+  const hardware_interface::HardwareInfo & info)
+{
+  if (configure_default(info) != hardware_interface::return_type::OK)
+  {
+    return hardware_interface::return_type::ERROR;
+  }
+
+  hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
+  hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
+  hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
+
+  for (const hardware_interface::ComponentInfo & joint : info_.joints)
+  {
+    // DiffBotSystem has exactly two states and one command interface on each joint
+    if (joint.command_interfaces.size() != 1)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("DiffBotSystemHardware"),
+        "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(),
+        joint.command_interfaces.size());
+      return hardware_interface::return_type::ERROR;
+    }
+
+    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("DiffBotSystemHardware"),
+        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
+        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
+      return hardware_interface::return_type::ERROR;
+    }
+
+    if (joint.state_interfaces.size() != 2)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("DiffBotSystemHardware"),
+        "Joint '%s' has %d state interface. 2 expected.", joint.name.c_str(),
+        joint.state_interfaces.size());
+      return hardware_interface::return_type::ERROR;
+    }
+
+    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("DiffBotSystemHardware"),
+        "Joint '%s' have '%s' as first state interface. '%s' and '%s' expected.",
+        joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
+        hardware_interface::HW_IF_POSITION);
+      return hardware_interface::return_type::ERROR;
+    }
+
+    if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY)
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("DiffBotSystemHardware"),
+        "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
+        joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
+      return hardware_interface::return_type::ERROR;
+    }
+  }
+
+  status_ = hardware_interface::status::CONFIGURED;
+  return hardware_interface::return_type::OK;
+}
+
+std::vector<hardware_interface::StateInterface> DiffBotSystemHardware::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  for (uint i = 0; i < info_.joints.size(); i++)
+  {
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_[i]));
+  }
+
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> DiffBotSystemHardware::export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+  for (uint i = 0; i < info_.joints.size(); i++)
+  {
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
+  }
+
+  return command_interfaces;
+}
+
+hardware_interface::return_type DiffBotSystemHardware::start()
+{
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Starting ...please wait...");
+
+  for (int i = 0; i <= hw_start_sec_; i++)
+  {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(
+      rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
+  }
+
+  // set some default values
+  for (uint i = 0; i < hw_states_.size(); i++)
+  {
+    if (std::isnan(hw_states_[i]))
+    {
+      hw_states_[i] = 0;
+      hw_commands_[i] = 0;
+    }
+  }
+
+  status_ = hardware_interface::status::STARTED;
+
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "System Successfully started!");
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type DiffBotSystemHardware::stop()
+{
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Stopping ...please wait...");
+
+  for (int i = 0; i <= hw_stop_sec_; i++)
+  {
+    rclcpp::sleep_for(std::chrono::seconds(1));
+    RCLCPP_INFO(
+      rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i);
+  }
+
+  status_ = hardware_interface::status::STOPPED;
+
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "System successfully stopped!");
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type DiffBotSystemHardware::read()
+{
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Reading...");
+
+  double tau = 0.2;
+  double a = tau / (tau + 1);
+  double b = 1 / (tau + 1);
+  for (uint i = 0; i < hw_commands_.size(); i++)
+  {
+    // Simulate DiffBot wheels's movement as a first-order system
+    hw_states_[1] = a * hw_states_[1] + b * hw_commands_[i];
+    hw_states_[0] += hw_states_[1] / 2;
+    RCLCPP_INFO(
+      rclcpp::get_logger("DiffBotSystemHardware"),
+      "Got position state %.5f and velocity state %.5f for '%s'!", hw_states_[0], hw_states_[1],
+      info_.joints[i].name.c_str());
+  }
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully read!");
+
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type ros2_control_demo_hardware::DiffBotSystemHardware::write()
+{
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing...");
+
+  for (uint i = 0; i < hw_commands_.size(); i++)
+  {
+    // Simulate sending commands to the hardware
+    RCLCPP_INFO(
+      rclcpp::get_logger("DiffBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i],
+      info_.joints[i].name.c_str());
+  }
+  RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully written!");
+
+  return hardware_interface::return_type::OK;
+}
+
+}  // namespace ros2_control_demo_hardware
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(
+  ros2_control_demo_hardware::DiffBotSystemHardware, hardware_interface::SystemInterface)

ros2_control_demo_robot/controllers/diffbot_diff_drive_controller.yaml

@@ -0,0 +1,66 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 2  # Hz
+
+    joint_state_controller:
+      type: joint_state_controller/JointStateController
+
+    diffbot_base_controller:
+      type: diff_drive_controller/DiffDriveController
+
+diffbot_base_controller:
+  ros__parameters:
+    left_wheel_names: ["left_wheel_joint"]
+    right_wheel_names: ["right_wheel_joint"]
+
+    wheel_separation: 0.10
+    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheel_radius: 0.015
+
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: base_link
+    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+    open_loop: true
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    #publish_limited_velocity: true
+    use_stamped_vel: false
+    #velocity_rolling_window_size: 10
+
+    # Preserve turning radius when limiting speed/acceleration/jerk
+    preserve_turning_radius: true
+
+    # Publish limited velocity
+    publish_cmd: true
+
+    # Publish wheel data
+    publish_wheel_data: true
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 1.0
+    angular.z.min_velocity: -1.0
+    angular.z.max_acceleration: 1.0
+    angular.z.min_acceleration: -1.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0