Unverified Commit 1e14150b authored by João Victor Torres Borges's avatar João Victor Torres Borges Committed by GitHub
Browse files

demo_diff_drive_controller (#70)



* demo_diff_drive_controller

* fix ament_flake errors

* applying review suggestions

* velocity and position models to simulate read hardware

* change folder to find packages

* Apply suggestions from code review

* adjust everywhere return_type

* wheels first-order dynamics

* remove slowdown parameter and uncrustify

* applying review suggestions

* Apply formatting
Co-authored-by: default avatarDenis Štogl <destogl@users.noreply.github.com>
Co-authored-by: default avatarDenis Štogl <denis@stogl.de>
parent f5d821c2
......@@ -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
)
......
// 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_
......@@ -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>
// 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)
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
<?xml version="1.0"?>
<!--
Copied and modified from ROS1 example:
https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/diffbot_description/urdf/diffbot.gazebo
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="diffbot_gazebo" params="prefix">
<!-- Link1 -->
<gazebo reference="${prefix}base_link">
<material>Gazebo/Blue</material>
</gazebo>
<!-- Link2 -->
<gazebo reference="${prefix}link1">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Black</material>
</gazebo>
<!-- Link3 -->
<gazebo reference="${prefix}link2">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Black</material>
</gazebo>
<!-- camera_link -->
<gazebo reference="${prefix}camera_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- camera -->
<gazebo reference="${prefix}camera_link">
<sensor type="camera" name="camera1">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>diffbot/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link_optical</frameName>
<!-- setting hackBaseline to anything but 0.0 will cause a misalignment
between the gazebo sensor image and the frame it is supposed to
be attached to -->
<hackBaseline>0.0</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0.0</Cx>
<Cy>0.0</Cy>
<focalLength>0.0</focalLength>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
<?xml version="1.0"?>
<!--
Copied from ROS1 example:
https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/materials.xacro
-->
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="diffbot" params="prefix">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="base_mass" value="0.3" /> <!-- arbitrary value for base mass -->
<xacro:property name="base_width" value="0.1" />
<xacro:property name="base_length" value="0.1" />
<xacro:property name="base_height" value="0.05" />
<xacro:property name="wheel_mass" value="0.3" /> <!-- arbitrary value for wheel mass -->
<xacro:property name="wheel_len" value="0.020" />
<xacro:property name="wheel_radius" value="0.015" />
<xacro:property name="caster_wheel_mass" value="0.1" /> <!-- arbitrary value for caster wheel mass -->
<xacro:property name="caster_wheel_radius" value="0.015" />
<xacro:property name="z_offset" value="-${base_height/2}" /> <!-- Space btw top of beam and the each joint -->
<!-- Base Link -->
<link name="${prefix}base_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${base_width} ${base_length} ${base_height}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${base_width} ${base_length} ${base_height}"/>
</geometry>
<material name="blue"/>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="${base_mass}"/>
<inertia
ixx="${base_mass / 12.0 * (base_length*base_length + base_height*base_height)}" ixy="0.0" ixz="0.0"
iyy="${base_mass / 12.0 * (base_height*base_height + base_width*base_width)}" iyz="0.0"
izz="${base_mass / 12.0 * (base_width*base_width + base_length*base_length)}"/>
</inertial>
</link>
<joint name="${prefix}left_wheel_joint" type="continuous">
<parent link="${prefix}base_link"/>
<child link="${prefix}left_wheel"/>
<origin xyz="0 -${base_width/2} ${z_offset}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.2"/>
</joint>
<!-- left wheel Link -->
<link name="${prefix}left_wheel">
<collision>
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
<geometry>
<cylinder length="${wheel_len}" radius="${wheel_radius}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
<geometry>
<cylinder length="${wheel_len}" radius="${wheel_radius}"/>
</geometry>
<material name="black"/>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
<mass value="${wheel_mass}"/>
<inertia
ixx="${wheel_mass / 12.0 * (3*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0"
izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/>
</inertial>
</link>
<joint name="${prefix}right_wheel_joint" type="continuous">
<parent link="${prefix}base_link"/>
<child link="${prefix}right_wheel"/>
<origin xyz="0 ${base_width/2} ${z_offset}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.2"/>