I have gone through and set up all the Exercises as guidance:
Here is my dynamixel_hardware_interface.cpp after all the exercises:
#include <algorithm>
#include <array>
#include <string>
#include <limits>
#include <vector>
#include "dynamixel_hardware_interface/dynamixel_hardware_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
namespace dynamixel_hardware
{
constexpr const char * kDynamixelHardware = "DynamixelHardware";
constexpr uint8_t kGoalPositionIndex = 0;
constexpr uint8_t kGoalVelocityIndex = 1;
constexpr uint8_t kPresentPositionVelocityCurrentIndex = 0;
constexpr const char * kGoalPositionItem = "Goal_Position";
constexpr const char * kGoalVelocityItem = "Goal_Velocity";
constexpr const char * kMovingSpeedItem = "Moving_Speed";
constexpr const char * kPresentPositionItem = "Present_Position";
constexpr const char * kPresentVelocityItem = "Present_Velocity";
constexpr const char * kPresentSpeedItem = "Present_Speed";
constexpr const char * kPresentCurrentItem = "Present_Current";
constexpr const char * kPresentLoadItem = "Present_Load";
CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo & info)
{
RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "configure");
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{
return CallbackReturn::ERROR;
}
joints_.resize(info_.joints.size(), Joint());
joint_ids_.resize(info_.joints.size(), 0);
for (uint i = 0; i < info_.joints.size(); i++) {
joint_ids_[i] = std::stoi(info_.joints[i].parameters.at("id"));
joints_[i].state.position = std::numeric_limits<double>::quiet_NaN();
joints_[i].state.velocity = std::numeric_limits<double>::quiet_NaN();
joints_[i].state.effort = std::numeric_limits<double>::quiet_NaN();
joints_[i].command.position = std::numeric_limits<double>::quiet_NaN();
joints_[i].command.velocity = std::numeric_limits<double>::quiet_NaN();
joints_[i].command.effort = std::numeric_limits<double>::quiet_NaN();
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]);
}
if (
info_.hardware_parameters.find("use_dummy") != info_.hardware_parameters.end() &&
info_.hardware_parameters.at("use_dummy") == "true") {
use_dummy_ = true;
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "dummy mode");
return CallbackReturn::SUCCESS;
}
auto usb_port = info_.hardware_parameters.at("usb_port");
auto baud_rate = std::stoi(info_.hardware_parameters.at("baud_rate"));
const char * log = nullptr;
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "usb_port: %s", usb_port.c_str());
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "baud_rate: %d", baud_rate);
if (!dynamixel_workbench_.init(usb_port.c_str(), baud_rate, &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return CallbackReturn::ERROR;
}
for (uint i = 0; i < info_.joints.size(); ++i) {
uint16_t model_number = 0;
if (!dynamixel_workbench_.ping(joint_ids_[i], &model_number, &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return CallbackReturn::ERROR;
}
}
enable_torque(false);
set_control_mode(ControlMode::Position, true);
enable_torque(true);
const ControlItem * goal_position =
dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalPositionItem);
if (goal_position == nullptr) {
return CallbackReturn::ERROR;
}
const ControlItem * goal_velocity =
dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalVelocityItem);
if (goal_velocity == nullptr) {
goal_velocity = dynamixel_workbench_.getItemInfo(joint_ids_[0], kMovingSpeedItem);
}
if (goal_velocity == nullptr) {
return CallbackReturn::ERROR;
}
const ControlItem * present_position =
dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentPositionItem);
if (present_position == nullptr) {
return CallbackReturn::ERROR;
}
const ControlItem * present_velocity =
dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentVelocityItem);
if (present_velocity == nullptr) {
present_velocity = dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentSpeedItem);
}
if (present_velocity == nullptr) {
return CallbackReturn::ERROR;
}
const ControlItem * present_current =
dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentCurrentItem);
if (present_current == nullptr) {
present_current = dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentLoadItem);
}
if (present_current == nullptr) {
return CallbackReturn::ERROR;
}
control_items_[kGoalPositionItem] = goal_position;
control_items_[kGoalVelocityItem] = goal_velocity;
control_items_[kPresentPositionItem] = present_position;
control_items_[kPresentVelocityItem] = present_velocity;
control_items_[kPresentCurrentItem] = present_current;
if (!dynamixel_workbench_.addSyncWriteHandler(
control_items_[kGoalPositionItem]->address, control_items_[kGoalPositionItem]->data_length,
&log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return CallbackReturn::ERROR;
}
if (!dynamixel_workbench_.addSyncWriteHandler(
control_items_[kGoalVelocityItem]->address, control_items_[kGoalVelocityItem]->data_length,
&log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return CallbackReturn::ERROR;
}
uint16_t start_address = std::min(
control_items_[kPresentPositionItem]->address, control_items_[kPresentCurrentItem]->address);
uint16_t read_length = control_items_[kPresentPositionItem]->data_length +
control_items_[kPresentVelocityItem]->data_length +
control_items_[kPresentCurrentItem]->data_length + 2;
if (!dynamixel_workbench_.addSyncReadHandler(start_address, read_length, &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> DynamixelHardware::export_state_interfaces()
{
RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "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, &joints_[i].state.position));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].state.velocity));
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].state.effort));
}
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> DynamixelHardware::export_command_interfaces()
{
RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "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_POSITION, &joints_[i].command.position));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].command.velocity));
}
return command_interfaces;
}
CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & previous_state)
{
RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "start");
for (uint i = 0; i < joints_.size(); i++) {
if (use_dummy_ && std::isnan(joints_[i].state.position)) {
joints_[i].state.position = 0.0;
joints_[i].state.velocity = 0.0;
joints_[i].state.effort = 0.0;
}
}
read();
reset_command();
write();
return CallbackReturn::SUCCESS;
}
CallbackReturn DynamixelHardware::on_deactivate(const rclcpp_lifecycle::State & previous_state)
{
RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "stop");
return CallbackReturn::SUCCESS;
}
return_type DynamixelHardware::read()
{
if (use_dummy_) {
return return_type::OK;
}
std::vector<uint8_t> ids(info_.joints.size(), 0);
std::vector<int32_t> positions(info_.joints.size(), 0);
std::vector<int32_t> velocities(info_.joints.size(), 0);
std::vector<int32_t> currents(info_.joints.size(), 0);
std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin());
const char * log = nullptr;
if (!dynamixel_workbench_.syncRead(
kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) {
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
if (!dynamixel_workbench_.getSyncReadData(
kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(),
control_items_[kPresentCurrentItem]->address,
control_items_[kPresentCurrentItem]->data_length, currents.data(), &log)) {
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
if (!dynamixel_workbench_.getSyncReadData(
kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(),
control_items_[kPresentVelocityItem]->address,
control_items_[kPresentVelocityItem]->data_length, velocities.data(), &log)) {
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
if (!dynamixel_workbench_.getSyncReadData(
kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(),
control_items_[kPresentPositionItem]->address,
control_items_[kPresentPositionItem]->data_length, positions.data(), &log)) {
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
for (uint i = 0; i < ids.size(); i++) {
joints_[i].state.position = dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]);
joints_[i].state.velocity = dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]);
joints_[i].state.effort = dynamixel_workbench_.convertValue2Current(currents[i]);
}
return return_type::OK;
}
return_type DynamixelHardware::write()
{
if (use_dummy_) {
for (auto & joint : joints_) {
joint.state.position = joint.command.position;
}
return return_type::OK;
}
std::vector<uint8_t> ids(info_.joints.size(), 0);
std::vector<int32_t> commands(info_.joints.size(), 0);
std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin());
const char * log = nullptr;
if (std::any_of(
joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.velocity != 0.0; })) {
// Velocity control
set_control_mode(ControlMode::Velocity);
for (uint i = 0; i < ids.size(); i++) {
commands[i] = dynamixel_workbench_.convertVelocity2Value(
ids[i], static_cast<float>(joints_[i].command.velocity));
}
if (!dynamixel_workbench_.syncWrite(
kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log)) {
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
return return_type::OK;
} else if (std::any_of(
joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != 0.0; })) {
// Effort control
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Effort control is not implemented");
return return_type::ERROR;
}
// Position control
set_control_mode(ControlMode::Position);
for (uint i = 0; i < ids.size(); i++) {
commands[i] = dynamixel_workbench_.convertRadian2Value(
ids[i], static_cast<float>(joints_[i].command.position));
}
if (!dynamixel_workbench_.syncWrite(
kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log)) {
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
return return_type::OK;
}
return_type DynamixelHardware::enable_torque(const bool enabled)
{
const char * log = nullptr;
if (enabled && !torque_enabled_) {
for (uint i = 0; i < info_.joints.size(); ++i) {
if (!dynamixel_workbench_.torqueOn(joint_ids_[i], &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return return_type::ERROR;
}
}
reset_command();
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Torque enabled");
} else if (!enabled && torque_enabled_) {
for (uint i = 0; i < info_.joints.size(); ++i) {
if (!dynamixel_workbench_.torqueOff(joint_ids_[i], &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return return_type::ERROR;
}
}
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Torque disabled");
}
torque_enabled_ = enabled;
return return_type::OK;
}
return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const bool force_set)
{
const char * log = nullptr;
if (mode == ControlMode::Velocity && (force_set || control_mode_ != ControlMode::Velocity)) {
bool torque_enabled = torque_enabled_;
if (torque_enabled) {
enable_torque(false);
}
for (uint i = 0; i < joint_ids_.size(); ++i) {
if (!dynamixel_workbench_.setVelocityControlMode(joint_ids_[i], &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return return_type::ERROR;
}
}
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control");
control_mode_ = ControlMode::Velocity;
if (torque_enabled) {
enable_torque(true);
}
} else if (
mode == ControlMode::Position && (force_set || control_mode_ != ControlMode::Position)) {
bool torque_enabled = torque_enabled_;
if (torque_enabled) {
enable_torque(false);
}
for (uint i = 0; i < joint_ids_.size(); ++i) {
if (!dynamixel_workbench_.setPositionControlMode(joint_ids_[i], &log)) {
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return return_type::ERROR;
}
}
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control");
control_mode_ = ControlMode::Position;
if (torque_enabled) {
enable_torque(true);
}
} else if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) {
RCLCPP_FATAL(
rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented");
return return_type::ERROR;
}
return return_type::OK;
}
return_type DynamixelHardware::reset_command()
{
for (uint i = 0; i < joints_.size(); i++) {
joints_[i].command.position = joints_[i].state.position;
joints_[i].command.velocity = 0.0;
joints_[i].command.effort = 0.0;
}
return return_type::OK;
}
} // namespace dynamixel_hardware
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(dynamixel_hardware::DynamixelHardware, hardware_interface::SystemInterface)
And below is the error from the terminal which make the package compiled unsuccessfully:
user:~/ros2_ws$ colcon build --symlink-install
[0.339s] WARNING:colcon.colcon_core.package_identification:Failed to parse ROS package manifest in 'src/dynamixel_hardware_interface': Error(s) in package 'src/dynamixel_hardware_interface/package.xml':
Invalid email "to-do" for person "user"
[0.469s] WARNING:colcon.colcon_core.verb:Some selected packages are already built in one or more underlay workspaces:
'diffbot_description' is in: /home/user/ros2_ws/install/diffbot_description
'diffbot_sim' is in: /home/user/ros2_ws/install/diffbot_sim
'dynamixel_sdk' is in: /home/user/ros2_ws/install/dynamixel_sdk
'dynamixel_sdk_custom_interfaces' is in: /home/user/ros2_ws/install/dynamixel_sdk_custom_interfaces
'joint_trajectory_publisher' is in: /home/user/ros2_ws/install/joint_trajectory_publisher
'my_robot_bringup' is in: /home/user/ros2_ws/install/my_robot_bringup
'my_robot_hardware_interface' is in: /home/user/ros2_ws/install/my_robot_hardware_interface
'rrbot_unit2' is in: /home/user/ros2_ws/install/rrbot_unit2
'rrbot_unit3' is in: /home/user/ros2_ws/install/rrbot_unit3
'rrbot_unit4' is in: /home/user/ros2_ws/install/rrbot_unit4
'rrbot_unit5' is in: /home/user/ros2_ws/install/rrbot_unit5
'solo_description' is in: /home/user/ros2_ws/install/solo_description
'spawn_robot' is in: /home/user/ros2_ws/install/spawn_robot
'spawn_solo' is in: /home/user/ros2_ws/install/spawn_solo
'dynamixel_sdk_examples' is in: /home/user/ros2_ws/install/dynamixel_sdk_examples
'dynamixel_workbench_toolbox' is in: /home/user/ros2_ws/install/dynamixel_workbench_toolbox
'dynamixel_hardware_interface' is in: /home/user/ros2_ws/install/dynamixel_hardware_interface
'dynamixel_workbench' is in: /home/user/ros2_ws/install/dynamixel_workbench
If a package in a merged underlay workspace is overridden and it installs headers, then all packages in the overlay must sort their include directories by workspace order. Failure to do so may result in build failures or undefined behavior at run time.
If the overridden package is used by another package in any underlay, then the overriding package in the overlay must be API and ABI compatible or undefined behavior at run time may occur.
If you understand the risks and want to override a package anyways, add the following to the command line:
--allow-overriding diffbot_description diffbot_sim dynamixel_hardware_interface dynamixel_sdk dynamixel_sdk_custom_interfaces dynamixel_sdk_examples dynamixel_workbench dynamixel_workbench_toolbox joint_trajectory_publisher my_robot_bringup my_robot_hardware_interface rrbot_unit2 rrbot_unit3 rrbot_unit4 rrbot_unit5 solo_description spawn_robot spawn_solo
This may be promoted to an error in a future release of colcon-core.
Starting >>> dynamixel_sdk
Starting >>> dynamixel_sdk_custom_interfaces
Starting >>> diffbot_description
Starting >>> diffbot_sim
Starting >>> joint_trajectory_publisher
Starting >>> my_robot_bringup
Starting >>> my_robot_hardware_interface
Starting >>> rrbot_unit2
Finished <<< diffbot_description [1.70s]
Starting >>> rrbot_unit3
Finished <<< my_robot_bringup [1.82s]
Finished <<< diffbot_sim [1.87s]
Starting >>> rrbot_unit4
Starting >>> rrbot_unit5
Finished <<< rrbot_unit2 [1.92s]
Starting >>> solo_description
Finished <<< dynamixel_sdk [2.30s]
Finished <<< my_robot_hardware_interface [2.20s]
Starting >>> dynamixel_workbench_toolbox
Starting >>> spawn_robot
Finished <<< rrbot_unit3 [1.50s]
Starting >>> spawn_solo
Finished <<< rrbot_unit5 [1.45s]
Finished <<< spawn_robot [1.13s]
Finished <<< solo_description [1.42s]
Finished <<< rrbot_unit4 [1.61s]
Finished <<< dynamixel_workbench_toolbox [1.52s]
Starting >>> dynamixel_hardware_interface
Starting >>> dynamixel_workbench
Finished <<< spawn_solo [0.89s]
Finished <<< dynamixel_sdk_custom_interfaces [4.33s]
Starting >>> dynamixel_sdk_examples
Finished <<< dynamixel_workbench [0.66s]
Finished <<< joint_trajectory_publisher [4.78s]
Finished <<< dynamixel_sdk_examples [0.71s]
--- stderr: dynamixel_hardware_interface
Error parsing '/home/user/ros2_ws/src/dynamixel_hardware_interface/package.xml':
Traceback (most recent call last):
File "/opt/ros/galactic/share/ament_cmake_core/cmake/core/package_xml_2_cmake.py", line 151, in <module>
main()
File "/opt/ros/galactic/share/ament_cmake_core/cmake/core/package_xml_2_cmake.py", line 54, in main
raise e
File "/opt/ros/galactic/share/ament_cmake_core/cmake/core/package_xml_2_cmake.py", line 50, in main
package = parse_package_string(
File "/usr/lib/python3/dist-packages/catkin_pkg/package.py", line 774, in parse_package_string
pkg.validate(warnings=warnings)
File "/usr/lib/python3/dist-packages/catkin_pkg/package.py", line 317, in validate
raise InvalidPackage('\n'.join(errors), self.filename)
catkin_pkg.package.InvalidPackage: Error(s) in package '/home/user/ros2_ws/src/dynamixel_hardware_interface/package.xml':
Invalid email "to-do" for person "user"
CMake Error at /opt/ros/galactic/share/ament_cmake_core/cmake/core/ament_package_xml.cmake:94 (message):
execute_process(/usr/bin/python3
/opt/ros/galactic/share/ament_cmake_core/cmake/core/package_xml_2_cmake.py
/home/user/ros2_ws/src/dynamixel_hardware_interface/package.xml
/home/user/ros2_ws/build/dynamixel_hardware_interface/ament_cmake_core/package.cmake)
returned error code 1
Call Stack (most recent call first):
/opt/ros/galactic/share/ament_cmake_core/cmake/core/ament_package_xml.cmake:49 (_ament_package_xml)
/opt/ros/galactic/share/ament_lint_auto/cmake/ament_lint_auto_find_test_dependencies.cmake:31 (ament_package_xml)
CMakeLists.txt:71 (ament_lint_auto_find_test_dependencies)
make: *** [Makefile:222: cmake_check_build_system] Error 1
---
Failed <<< dynamixel_hardware_interface [2.53s, exited with code 2]
Summary: 17 packages finished [6.67s]
1 package failed: dynamixel_hardware_interface
1 package had stderr output: dynamixel_hardware_interface
user:~/ros2_ws$