rrobot_with_rrbot_controlller.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, TimerAction, LogInfo
from launch.event_handlers import OnProcessExit, OnExecutionComplete
from launch.launch_description_sources import PythonLaunchDescriptionSource
import xacro
def generate_launch_description():
rrbot_description_path = os.path.join(
get_package_share_directory('rrbot_unit2'))
xacro_file = os.path.join(rrbot_description_path,
'urdf',
'rrbot.xacro')
doc = xacro.parse(open(xacro_file))
xacro.process_doc(doc)
robot_description_config = doc.toxml()
robot_description = {'robot_description': robot_description_config}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description]
)
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'robot'],
output='screen')
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
robot_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["rrbot_controller", "-c", "/controller_manager"],
)
return LaunchDescription([
RegisterEventHandler(
OnExecutionComplete(
target_action=spawn_entity,
on_completion=[
LogInfo(
msg='Spawn finished, waiting 10 seconds to start controllers.'),
TimerAction(
period=10.0,
actions=[joint_state_broadcaster_spawner],
)
]
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[robot_controller_spawner],
)
),
spawn_entity,
node_robot_state_publisher,
])
rrbot_controllers_custom.yaml:
Controller manager configuration
controller_manager:
ros__parameters:
update_rate: 50 # Hz
# Define a name for controllers that we plan to use
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
rrbot_controller:
type: rrbot_controller/RRBotController
Properties of the custom controler and definition of joints to use
rrbot_controller:
ros__parameters:
joints:
- joint1
- joint2
interface_name: position
RRbot Xacro:
All of the code is the same as in the unit2 robot what was changed is that the gazebo plugin line is only this:
gazebo_ros2_control/GazeboSystem
$(find rrbot_controller)/config/rrbot_controllers_custom.yaml
rrbot_controller.cpp:
#include “rrbot_controller/rrbot_controller.hpp”
#include
#include
#include
#include
namespace rrbot_controller
{
RRBotController::RRBotController() : controller_interface::ControllerInterface() {}
CallbackReturn RRBotController::on_init() {
try {
auto_declare(“joints”, std::vectorstd::string());
auto_declare(“interface_name”, std::string());
} catch (const std::exception &e) {
fprintf(stderr, “Exception thrown during init stage with message: %s \n”,
e.what());
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
CallbackReturn RRBotController::on_configure(
const rclcpp_lifecycle::State & /previous_state/) {
auto error_if_empty = [&](const auto ¶meter, const char *parameter_name) {
if (parameter.empty()) {
RCLCPP_ERROR(get_node()->get_logger(), “‘%s’ parameter was empty”,
parameter_name);
return true;
}
return false;
};
auto get_string_array_param_and_error_if_empty =
[&](std::vectorstd::string ¶meter, const char *parameter_name) {
parameter = get_node()->get_parameter(parameter_name).as_string_array();
return error_if_empty(parameter, parameter_name);
};
auto get_string_param_and_error_if_empty =
[&](std::string ¶meter, const char *parameter_name) {
parameter = get_node()->get_parameter(parameter_name).as_string();
return error_if_empty(parameter, parameter_name);
};
if (
get_string_array_param_and_error_if_empty(joint_names_, “joints”) ||
get_string_param_and_error_if_empty(interface_name_, “interface_name”)) {
return CallbackReturn::ERROR;
}
// Command Subscriber and callbacks
auto callback_command =
[&](const std::shared_ptr msg) → void {
if (msg->joint_names.size() == joint_names_.size()) {
input_command_.writeFromNonRT(msg);
} else {
RCLCPP_ERROR(get_node()->get_logger(),
"Received %zu , but expected %zu joints in command. "
“Ignoring message.”,
msg->joint_names.size(), joint_names_.size());
}
};
command_subscriber_ = get_node()->create_subscription(
“~/commands”, rclcpp::SystemDefaultsQoS(), callback_command);
// State publisher
s_publisher_ =
get_node()->create_publisher(
“~/state”, rclcpp::SystemDefaultsQoS());
state_publisher_ = std::make_unique(s_publisher_);
state_publisher_->lock();
state_publisher_->msg_.header.frame_id = joint_names_[0];
state_publisher_->unlock();
RCLCPP_INFO_STREAM(get_node()->get_logger(), “configure successful”);
return CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration
RRBotController::command_interface_configuration() const {
controller_interface::InterfaceConfiguration command_interfaces_config;
command_interfaces_config.type =
controller_interface::interface_configuration_type::INDIVIDUAL;
command_interfaces_config.names.reserve(joint_names_.size());
for (const auto &joint : joint_names_) {
command_interfaces_config.names.push_back(joint + “/” + interface_name_);
}
return command_interfaces_config;
}
template
bool get_ordered_interfaces(
std::vector &unordered_interfaces,
const std::vectorstd::string &joint_names,
const std::string &interface_type,
std::vector<std::reference_wrapper> &ordered_interfaces) {
for (const auto &joint_name : joint_names) {
for (auto &command_interface : unordered_interfaces) {
if ((command_interface.get_name() == joint_name) &&
(command_interface.get_interface_name() == interface_type)) {
ordered_interfaces.push_back(std::ref(command_interface));
}
}
}
return joint_names.size() == ordered_interfaces.size();
}
CallbackReturn RRBotController::on_activate(
const rclcpp_lifecycle::State & /previous_state/) {
// Set default value in command
std::shared_ptr msg =
std::make_shared();
msg->joint_names = joint_names_;
msg->displacements.resize(joint_names_.size(),
std::numeric_limits::quiet_NaN());
input_command_.writeFromNonRT(msg);
return CallbackReturn::SUCCESS;
}
CallbackReturn RRBotController::on_deactivate(
const rclcpp_lifecycle::State & /previous_state/) {
return CallbackReturn::SUCCESS;
}
controller_interface::return_type
RRBotController::update(const rclcpp::Time &time,
const rclcpp::Duration & /period/) {
auto current_command = input_command_.readFromRT();
for (size_t i = 0; i < command_interfaces_.size(); ++i) {
if (!std::isnan((*current_command)->displacements[i])) {
command_interfaces_[i].set_value((*current_command)->displacements[i]);
}
}
if (state_publisher_ && state_publisher_->trylock()) {
state_publisher_->msg_.header.stamp = time;
state_publisher_->msg_.set_point = command_interfaces_[0].get_value();
state_publisher_->unlockAndPublish();
}
return controller_interface::return_type::OK;
}
} // namespace rrbot_controller
#include “pluginlib/class_list_macros.hpp”
PLUGINLIB_EXPORT_CLASS(rrbot_controller::RRBotController, controller_interface::ControllerInterface)
