diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 25ed3def2..92550edcc 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/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 ) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp new file mode 100644 index 000000000..53ee709c7 --- /dev/null +++ b/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 +#include +#include + +#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 +{ +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 export_state_interfaces() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + std::vector 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 hw_commands_; + std::vector hw_states_; +}; + +} // namespace ros2_control_demo_hardware + +#endif // ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_ diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index d7495ce54..c2e351943 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/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. + + + 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. + + diff --git a/ros2_control_demo_hardware/src/diffbot_system.cpp b/ros2_control_demo_hardware/src/diffbot_system.cpp new file mode 100644 index 000000000..7e74f1f02 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::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 DiffBotSystemHardware::export_state_interfaces() +{ + std::vector 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 DiffBotSystemHardware::export_command_interfaces() +{ + std::vector 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) diff --git a/ros2_control_demo_robot/controllers/diffbot_diff_drive_controller.yaml b/ros2_control_demo_robot/controllers/diffbot_diff_drive_controller.yaml new file mode 100644 index 000000000..1ab715264 --- /dev/null +++ b/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 diff --git a/ros2_control_demo_robot/description/diffbot/diffbot.gazebo.xacro b/ros2_control_demo_robot/description/diffbot/diffbot.gazebo.xacro new file mode 100644 index 000000000..1f2073d4b --- /dev/null +++ b/ros2_control_demo_robot/description/diffbot/diffbot.gazebo.xacro @@ -0,0 +1,86 @@ + + + + + + + + + Gazebo/Blue + + + + + 0.2 + 0.2 + Gazebo/Black + + + + + 0.2 + 0.2 + Gazebo/Black + + + + + 0.2 + 0.2 + Gazebo/Red + + + + + + 30.0 + + 1.3962634 + + 800 + 800 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + 0.0 + diffbot/camera1 + image_raw + camera_info + camera_link_optical + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0 + 0.0 + 0.0 + 0.0 + + + + + + + diff --git a/ros2_control_demo_robot/description/diffbot/diffbot.materials.xacro b/ros2_control_demo_robot/description/diffbot/diffbot.materials.xacro new file mode 100644 index 000000000..035bf5822 --- /dev/null +++ b/ros2_control_demo_robot/description/diffbot/diffbot.materials.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_control_demo_robot/description/diffbot/diffbot.urdf.xacro b/ros2_control_demo_robot/description/diffbot/diffbot.urdf.xacro new file mode 100644 index 000000000..2edf31fce --- /dev/null +++ b/ros2_control_demo_robot/description/diffbot/diffbot.urdf.xacro @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_control_demo_robot/description/diffbot/diffbot_system.ros2_control.xacro b/ros2_control_demo_robot/description/diffbot/diffbot_system.ros2_control.xacro new file mode 100644 index 000000000..59c4af134 --- /dev/null +++ b/ros2_control_demo_robot/description/diffbot/diffbot_system.ros2_control.xacro @@ -0,0 +1,32 @@ + + + + + + + + ros2_control_demo_hardware/DiffBotSystemHardware + 2.0 + 3.0 + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + + + diff --git a/ros2_control_demo_robot/description/diffbot_system.urdf.xacro b/ros2_control_demo_robot/description/diffbot_system.urdf.xacro new file mode 100644 index 000000000..7041cf01e --- /dev/null +++ b/ros2_control_demo_robot/description/diffbot_system.urdf.xacro @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_control_demo_robot/launch/diffbot_system.launch.py b/ros2_control_demo_robot/launch/diffbot_system.launch.py new file mode 100755 index 000000000..3e07bfb38 --- /dev/null +++ b/ros2_control_demo_robot/launch/diffbot_system.launch.py @@ -0,0 +1,67 @@ +# Copyright 2020 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_path = os.path.join( + get_package_share_directory("ros2_control_demo_robot"), + "description", + "diffbot_system.urdf.xacro", + ) + robot_description_config = xacro.process_file(robot_description_path) + robot_description = {"robot_description": robot_description_config.toxml()} + + diffbot_diff_drive_controller = os.path.join( + get_package_share_directory("ros2_control_demo_robot"), + "controllers", + "diffbot_diff_drive_controller.yaml", + ) + + return LaunchDescription( + [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[robot_description], + ), + Node( + package="joint_state_publisher", + executable="joint_state_publisher", + name="joint_state_publisher", + output="screen", + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, diffbot_diff_drive_controller], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ), + ] + )