Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

adhere to style guide #73

Merged
merged 1 commit into from
Apr 23, 2024
Merged

adhere to style guide #73

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion dynamixel_hardware/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ ament_target_dependencies(
hardware_interface
pluginlib
dynamixel_workbench_toolbox
)
)


pluginlib_export_plugin_description_file(hardware_interface dynamixel_hardware.xml)
Expand Down
12 changes: 6 additions & 6 deletions dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,14 @@

#include <dynamixel_workbench_toolbox/dynamixel_workbench.h>

#include <map>
#include <vector>

#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <rclcpp_lifecycle/state.hpp>

#include <map>
#include <vector>

#include "dynamixel_hardware/visiblity_control.h"
#include "rclcpp/macros.hpp"

Expand All @@ -47,7 +47,8 @@ struct Joint
JointValue prev_command{};
};

enum class ControlMode {
enum class ControlMode
{
Position,
Velocity,
Torque,
Expand All @@ -58,8 +59,7 @@ enum class ControlMode {
PWM,
};

class DynamixelHardware
: public hardware_interface::SystemInterface
class DynamixelHardware : public hardware_interface::SystemInterface
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardware)
Expand Down
125 changes: 74 additions & 51 deletions dynamixel_hardware/src/dynamixel_hardware.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,7 @@ constexpr const char * const kExtraJointParameters[] = {
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)
{
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
return CallbackReturn::ERROR;
}

Expand All @@ -75,7 +74,8 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo

if (
info_.hardware_parameters.find("use_dummy") != info_.hardware_parameters.end() &&
info_.hardware_parameters.at("use_dummy") == "true") {
info_.hardware_parameters.at("use_dummy") == "true")
{
use_dummy_ = true;
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "dummy mode");
return CallbackReturn::SUCCESS;
Expand Down Expand Up @@ -152,24 +152,26 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
control_items_[kPresentCurrentItem] = present_current;

if (!dynamixel_workbench_.addSyncWriteHandler(
control_items_[kGoalPositionItem]->address, control_items_[kGoalPositionItem]->data_length,
&log)) {
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)) {
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;
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;
Expand All @@ -183,12 +185,15 @@ std::vector<hardware_interface::StateInterface> DynamixelHardware::export_state_
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));
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;
Expand All @@ -199,10 +204,12 @@ std::vector<hardware_interface::CommandInterface> DynamixelHardware::export_comm
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));
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;
Expand All @@ -225,13 +232,16 @@ CallbackReturn DynamixelHardware::on_activate(const rclcpp_lifecycle::State & /*
return CallbackReturn::SUCCESS;
}

CallbackReturn DynamixelHardware::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */)
CallbackReturn DynamixelHardware::on_deactivate(
const rclcpp_lifecycle::State & /* previous_state */)
{
RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "stop");
return CallbackReturn::SUCCESS;
}

return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
return_type DynamixelHardware::read(
const rclcpp::Time & /* time */,
const rclcpp::Duration & /* period */)
{
if (use_dummy_) {
return return_type::OK;
Expand All @@ -246,28 +256,32 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp
const char * log = nullptr;

if (!dynamixel_workbench_.syncRead(
kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), &log)) {
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)) {
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)) {
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)) {
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);
}

Expand All @@ -280,7 +294,9 @@ return_type DynamixelHardware::read(const rclcpp::Time & /* time */, const rclcp
return return_type::OK;
}

return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
return_type DynamixelHardware::write(
const rclcpp::Time & /* time */,
const rclcpp::Duration & /* period */)
{
if (use_dummy_) {
for (auto & joint : joints_) {
Expand All @@ -292,22 +308,26 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc

// Velocity control
if (std::any_of(
joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.velocity != j.prev_command.velocity; })) {
joints_.cbegin(), joints_.cend(), [](auto j) {
return j.command.velocity != j.prev_command.velocity;
}))
{
set_control_mode(ControlMode::Velocity);
if(mode_changed_)
{
if (mode_changed_) {
set_joint_params();
}
set_joint_velocities();
return return_type::OK;
}

// Position control
if (std::any_of(
joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.position != j.prev_command.position; })) {
joints_.cbegin(), joints_.cend(), [](auto j) {
return j.command.position != j.prev_command.position;
}))
{
set_control_mode(ControlMode::Position);
if(mode_changed_)
{
if (mode_changed_) {
set_joint_params();
}
set_joint_positions();
Expand All @@ -316,12 +336,14 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc

// Effort control
if (std::any_of(
joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.effort != 0.0; })) {
joints_.cbegin(), joints_.cend(), [](auto j) {return j.command.effort != 0.0;}))
{
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Effort control is not implemented");
return return_type::ERROR;
}

// if all command values are unchanged, then remain in existing control mode and set corresponding command values
// If all command values are unchanged, then remain in existing control mode and set
// corresponding command values
switch (control_mode_) {
case ControlMode::Velocity:
set_joint_velocities();
Expand All @@ -331,12 +353,11 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc
set_joint_positions();
return return_type::OK;
break;
default: // effort, etc
default: // effort, etc
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "Control mode not implemented");
return return_type::ERROR;
break;
}

}

return_type DynamixelHardware::enable_torque(const bool enabled)
Expand Down Expand Up @@ -384,8 +405,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
}
}
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control");
if(control_mode_ != ControlMode::Velocity)
{
if (control_mode_ != ControlMode::Velocity) {
mode_changed_ = true;
control_mode_ = ControlMode::Velocity;
}
Expand All @@ -409,8 +429,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
}
}
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control");
if(control_mode_ != ControlMode::Position)
{
if (control_mode_ != ControlMode::Position) {
mode_changed_ = true;
control_mode_ = ControlMode::Position;
}
Expand All @@ -420,7 +439,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
}
return return_type::OK;
}

if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) {
RCLCPP_FATAL(
rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented");
Expand Down Expand Up @@ -457,7 +476,8 @@ CallbackReturn DynamixelHardware::set_joint_positions()
ids[i], static_cast<float>(joints_[i].command.position));
}
if (!dynamixel_workbench_.syncWrite(
kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log)) {
kGoalPositionIndex, ids.data(), ids.size(), commands.data(), 1, &log))
{
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
return CallbackReturn::SUCCESS;
Expand All @@ -476,7 +496,8 @@ CallbackReturn DynamixelHardware::set_joint_velocities()
ids[i], static_cast<float>(joints_[i].command.velocity));
}
if (!dynamixel_workbench_.syncWrite(
kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log)) {
kGoalVelocityIndex, ids.data(), ids.size(), commands.data(), 1, &log))
{
RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log);
}
return CallbackReturn::SUCCESS;
Expand All @@ -493,7 +514,9 @@ CallbackReturn DynamixelHardware::set_joint_params()
RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
return CallbackReturn::ERROR;
}
RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "%s set to %d for joint %d", paramName, value, i);
RCLCPP_INFO(
rclcpp::get_logger(
kDynamixelHardware), "%s set to %d for joint %d", paramName, value, i);
}
}
}
Expand Down
Loading