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

Emulate button, cleanup parameters and logging #25

Merged
merged 8 commits into from
Aug 20, 2024
Merged

Emulate button, cleanup parameters and logging #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
a7a5c48
add debug logs
Aug 19, 2024
5203bc5
add parameters for emulate_button and interface_id
Aug 20, 2024
75e879b
make fd_inertia a parameter also
Aug 20, 2024
7c59dff
emulate button
Aug 20, 2024
347d9e1
update description file
Aug 20, 2024
00a3875
linting
Aug 20, 2024
71cbd1e
cleanup logging
Aug 20, 2024
f76f208
emulate_button false by default
Aug 20, 2024
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
3 changes: 3 additions & 0 deletions fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,9 @@ controller_interface::return_type FdClutchBroadcaster::update(
const rclcpp::Time & /*time*/,
const rclcpp::Duration & /*period*/)
{
RCLCPP_DEBUG(get_node()->get_logger(), "Entering update()");
if (realtime_clutch_publisher_ && realtime_clutch_publisher_->trylock()) {
RCLCPP_DEBUG(get_node()->get_logger(), "Lock acquired");
// Read provided state interface
bool is_interface_a_button = get_node()->get_parameter("is_interface_a_button").as_bool();
double read_value = state_interfaces_[0].get_value();
Expand All @@ -148,6 +150,7 @@ controller_interface::return_type FdClutchBroadcaster::update(
// Publish clucth
auto & fd_clutch_msg = realtime_clutch_publisher_->msg_;
fd_clutch_msg.data = clutch_state;
RCLCPP_DEBUG(get_node()->get_logger(), "publish and unlock");
realtime_clutch_publisher_->unlockAndPublish();
}

Expand Down
6 changes: 6 additions & 0 deletions fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,6 +141,7 @@ FdInertiaBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_st
Eigen::Vector3d trans = Eigen::Vector3d::Zero();

if (transform_trans_param.size() == 0) {
RCLCPP_INFO(get_node()->get_logger(), "No (linear) transformation provided. Using t = 0,0,0");
trans << 0.0, 0.0, 0.0;
} else if (transform_trans_param.size() == 3) {
trans << transform_trans_param[0], transform_trans_param[1], transform_trans_param[2];
Expand All @@ -151,6 +152,8 @@ FdInertiaBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_st

if (transform_rot_param.size() == 0) {
q = Eigen::Quaternion<double>(1, 0, 0, 0);
RCLCPP_INFO(get_node()->get_logger(),
"No (angular) transformation provided. Using q = 1,0,0,0");
} else if (transform_rot_param.size() == 3) {
Eigen::AngleAxisd rollAngle(transform_rot_param[0], Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd yawAngle(transform_rot_param[1], Eigen::Vector3d::UnitY());
Expand Down Expand Up @@ -212,7 +215,9 @@ controller_interface::return_type FdInertiaBroadcaster::update(
const rclcpp::Time & /*time*/,
const rclcpp::Duration & /*period*/)
{
RCLCPP_DEBUG(get_node()->get_logger(), "Entering update()");
if (realtime_inertia_publisher_ && realtime_inertia_publisher_->trylock()) {
RCLCPP_DEBUG(get_node()->get_logger(), "Lock acquired");
inertia_in_base_ = Eigen::Matrix<double, 6, 6>::Zero();

// Map upper triangular part of inertia to inertia state interface
Expand All @@ -234,6 +239,7 @@ controller_interface::return_type FdInertiaBroadcaster::update(
// Publish inertia
auto & fd_inertia_msg = realtime_inertia_publisher_->msg_;
matrixEigenToMsg(inertia_, fd_inertia_msg);
RCLCPP_DEBUG(get_node()->get_logger(), "publish and unlock");
realtime_inertia_publisher_->unlockAndPublish();
}

Expand Down
8 changes: 6 additions & 2 deletions fd_description/ros2_control/fd.r2c_hardware.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,18 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="fd_ros2_control" params="robot_id plugin_name:='FDHapticInterface' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false'">
<xacro:macro
name="fd_ros2_control"
params="robot_id plugin_name:='FDHapticInterface' interface_id:='-1' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false' emulate_button:='false'">
<ros2_control name="${plugin_name}" type="system">
<hardware>
<xacro:if value="${use_fake_hardware}">
<plugin>fake_components/GenericSystem</plugin>
</xacro:if>
<xacro:unless value="${use_fake_hardware}">
<plugin>fd_hardware/FDEffortHardwareInterface</plugin>
<param name="interface_ID">-1</param>
<param name="interface_id">${interface_id}</param>
<param name="emulate_button">${emulate_button}</param>
<param name="inertia_interface_name">fd_inertia</param>
</xacro:unless>
</hardware>

Expand Down
10 changes: 8 additions & 2 deletions fd_hardware/include/fd_hardware/fd_effort_hi.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,11 +59,17 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface
hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;

private:
// ID of the interface (Rq: "-1" = invalid/any that is connected)
/// ID of the interface (Rq: "-1" = invalid/any that is connected)
char interface_ID_ = -1;
// Turned to true after the connection

/// Turned to true after the connection
bool isConnected_ = false;

/// If true, the button will be emulated from clutch joint (for omega 6 / sigma 7, see SDK doc)
bool emulate_button_ = false;

std::string inertia_interface_name_;

// Store the command for the robot
std::vector<double> hw_commands_effort_;
std::vector<double> hw_states_position_;
Expand Down
96 changes: 64 additions & 32 deletions fd_hardware/src/fd_effort_hi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
#include "fd_sdk_vendor/dhd.hpp"
#include "fd_sdk_vendor/drd.hpp"

#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/lexical_casts.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

Expand All @@ -36,6 +38,8 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
namespace fd_hardware
{

rclcpp::Logger LOGGER = rclcpp::get_logger("FDEffortHardwareInterface");

unsigned int flattened_index_from_triangular_index(unsigned int i, unsigned int j)
{
return i * (i - 1) / 2 + j;
Expand Down Expand Up @@ -70,45 +74,45 @@ CallbackReturn FDEffortHardwareInterface::on_init(
// PHI has currently exactly 3 states and 1 command interface on each joint
if (joint.command_interfaces.size() != 1) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Joint '%s' has %lu command interfaces found. 1 expected.", joint.name.c_str(),
joint.command_interfaces.size());
return CallbackReturn::ERROR;
}

if (joint.command_interfaces[0].name != hardware_interface::HW_IF_EFFORT) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
return CallbackReturn::ERROR;
}

if (joint.state_interfaces.size() != 3) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Joint '%s' has %ld state interface. 3 expected.", joint.name.c_str(),
joint.state_interfaces.size());
return CallbackReturn::ERROR;
}

if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
return CallbackReturn::ERROR;
}
if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
return CallbackReturn::ERROR;
}
if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
return CallbackReturn::ERROR;
Expand All @@ -117,14 +121,14 @@ CallbackReturn FDEffortHardwareInterface::on_init(
for (const hardware_interface::ComponentInfo & button : info_.gpios) {
if (button.state_interfaces.size() != 1) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Button '%s' has %lu state interface. 1 expected.", button.name.c_str(),
button.state_interfaces.size());
return CallbackReturn::ERROR;
}
if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Button '%s' have %s state interface. '%s' expected.", button.name.c_str(),
button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
return CallbackReturn::ERROR;
Expand All @@ -133,20 +137,44 @@ CallbackReturn FDEffortHardwareInterface::on_init(
for (const hardware_interface::ComponentInfo & button : info_.gpios) {
if (button.state_interfaces.size() != 1) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Button '%s' has %lu state interface. 1 expected.", button.name.c_str(),
button.state_interfaces.size());
return CallbackReturn::ERROR;
}
if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
LOGGER,
"Button '%s' have %s state interface. '%s' expected.", button.name.c_str(),
button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
return CallbackReturn::ERROR;
}
}


// Get parameters
auto it_interface_id = info_.hardware_parameters.find("interface_id");
if (it_interface_id != info_.hardware_parameters.end()) {
interface_ID_ = stoi(it_interface_id->second);
RCLCPP_INFO(LOGGER, "Using interface ID: %d", interface_ID_);
} else {
interface_ID_ = -1;
}

auto it_emulate_button = info_.hardware_parameters.find("emulate_button");
if (it_emulate_button != info_.hardware_parameters.end()) {
emulate_button_ = hardware_interface::parse_bool(it_emulate_button->second);
} else {
emulate_button_ = false;
}

auto it_fd_inertia = info_.hardware_parameters.find("inertia_interface_name");
if (it_fd_inertia != info_.hardware_parameters.end()) {
inertia_interface_name_ = it_fd_inertia->second;
} else {
inertia_interface_name_ = "fd_inertia";
}

return CallbackReturn::SUCCESS;
}
// ------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -175,15 +203,12 @@ FDEffortHardwareInterface::export_state_interfaces()
info_.gpios[i].name, hardware_interface::HW_IF_POSITION, &hw_button_state_[i]));
}

// TODO(tpoignonec) Make a parameter or somehow retrieve robot prefix
std::string inertia_interface_name = "fd_inertia";

// Map upper triangular part of inertia to inertia state interface
for (uint row = 0; row < 6; row++) {
for (uint col = row; col < 6; col++) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
inertia_interface_name,
inertia_interface_name_,
std::to_string(row) + "" + std::to_string(col),
&hw_states_inertia_[flattened_index_from_triangular_index(row, col)]));
}
Expand All @@ -210,15 +235,13 @@ CallbackReturn FDEffortHardwareInterface::on_activate(
{
(void)previous_state; // hush "-Wunused-parameter" warning

RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "Starting ...please wait...");
RCLCPP_INFO(LOGGER, "Starting ...please wait...");

if (connectToDevice()) {
RCLCPP_INFO(
rclcpp::get_logger("FDEffortHardwareInterface"), "System Successfully started!");
RCLCPP_INFO(LOGGER, "System Successfully started!");
return CallbackReturn::SUCCESS;
} else {
RCLCPP_ERROR(
rclcpp::get_logger("FDEffortHardwareInterface"), "System Not started!");
RCLCPP_ERROR(LOGGER, "System Not started!");
return CallbackReturn::ERROR;
}
}
Expand All @@ -227,15 +250,13 @@ CallbackReturn FDEffortHardwareInterface::on_deactivate(
const rclcpp_lifecycle::State & previous_state)
{
(void)previous_state; // hush "-Wunused-parameter" warning
RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "Stopping ...please wait...");
RCLCPP_INFO(LOGGER, "Stopping ...please wait...");

if (disconnectFromDevice()) {
RCLCPP_INFO(
rclcpp::get_logger("FDEffortHardwareInterface"), "System successfully stopped!");
RCLCPP_INFO(LOGGER, "System successfully stopped!");
return CallbackReturn::SUCCESS;
} else {
RCLCPP_ERROR(
rclcpp::get_logger("FDEffortHardwareInterface"), "System Not stopped!");
RCLCPP_ERROR(LOGGER, "System Not stopped!");
return CallbackReturn::ERROR;
}
}
Expand Down Expand Up @@ -318,7 +339,7 @@ hardware_interface::return_type FDEffortHardwareInterface::read(
if (flag >= 0) {
return hardware_interface::return_type::OK;
} else {
RCLCPP_ERROR(rclcpp::get_logger("FDEffortHardwareInterface"), "Updating from system failed!");
RCLCPP_ERROR(LOGGER, "Updating from system failed!");
return hardware_interface::return_type::ERROR;
}
}
Expand Down Expand Up @@ -372,9 +393,9 @@ bool FDEffortHardwareInterface::connectToDevice()

// Check if the device has 3 dof or more
if (dhdHasWrist(interface_ID_)) {
RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Rotation enabled ");
RCLCPP_INFO(LOGGER, "dhd : Rotation enabled ");
} else {
RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Rotation disabled ");
RCLCPP_INFO(LOGGER, "dhd : Rotation disabled ");
}

// Retrieve the mass of the device
Expand Down Expand Up @@ -411,9 +432,20 @@ bool FDEffortHardwareInterface::connectToDevice()
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Gravity compensation enabled");
}
RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Device connected !");

RCLCPP_INFO(LOGGER, "dhd : Device connected !");

if (emulate_button_ && dhdHasGripper(interface_ID_)) {
RCLCPP_INFO(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Emulating button from clutch joint !");
if (dhdEmulateButton(DHD_ON, interface_ID_) < DHD_NO_ERROR) {
RCLCPP_WARN(
rclcpp::get_logger(
"FDEffortHardwareInterface"), "dhd : Could not enable button emulation!");
disconnectFromDevice();
}
}
// Set force to zero
if (dhdSetForceAndTorqueAndGripperForce(
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
interface_ID_) < DHD_NO_ERROR)
Expand All @@ -437,7 +469,7 @@ bool FDEffortHardwareInterface::connectToDevice()
// ------------------------------------------------------------------------------------------
bool FDEffortHardwareInterface::disconnectFromDevice()
{
RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : stopping the device.");
RCLCPP_INFO(LOGGER, "dhd : stopping the device.");
// Stop the device: disables the force on the haptic device and puts it into BRAKE mode.
int hasStopped = -1;
while (hasStopped < 0) {
Expand All @@ -449,11 +481,11 @@ bool FDEffortHardwareInterface::disconnectFromDevice()
// close device connection
int connectionIsClosed = dhdClose(interface_ID_);
if (connectionIsClosed >= 0) {
RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Disconnected ! ");
RCLCPP_INFO(LOGGER, "dhd : Disconnected ! ");
interface_ID_ = false;
return true;
} else {
RCLCPP_ERROR(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Failed to disconnect !");
RCLCPP_ERROR(LOGGER, "dhd : Failed to disconnect !");
return false;
}
}
Expand Down
Loading