Skip to content

Commit

Permalink
Merge branch 'ICube-Robotics:main' into disconnect_when_ctrlc
Browse files Browse the repository at this point in the history
  • Loading branch information
CalaW authored Jan 27, 2024
2 parents 6b289d9 + d1b5507 commit 14ad4ca
Show file tree
Hide file tree
Showing 10 changed files with 193 additions and 18 deletions.
2 changes: 1 addition & 1 deletion fd_bringup/launch/fd.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ def generate_launch_description():

# Load controllers
load_controllers = []
for controller in ['fd_controller', 'joint_state_broadcaster']:
for controller in ['fd_controller', 'joint_state_broadcaster', 'fd_ee_broadcaster']:
load_controllers += [
Node(
package='controller_manager',
Expand Down
1 change: 1 addition & 0 deletions fd_bringup/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
<exec_depend>xacro</exec_depend>
<exec_depend>controller_manager</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>effort_controllers</exec_depend>
<exec_depend>fd_description</exec_depend>
<exec_depend>fd_hardware</exec_depend>
<exec_depend>urdf</exec_depend>
Expand Down
3 changes: 3 additions & 0 deletions fd_controllers/ee_pose_broadcaster/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED)
find_package(rcutils REQUIRED)
find_package(realtime_tools REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3)

Expand All @@ -33,6 +34,7 @@ ament_target_dependencies(ee_pose_broadcaster
rcutils
realtime_tools
geometry_msgs
std_msgs
eigen3_cmake_module
Eigen3
)
Expand Down Expand Up @@ -62,6 +64,7 @@ ament_export_dependencies(
geometry_msgs
eigen3_cmake_module
Eigen3
example_interfaces
)
ament_export_include_directories(
include
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "realtime_tools/realtime_publisher.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "std_msgs/msg/bool.hpp"

namespace ee_pose_broadcaster
{
Expand Down Expand Up @@ -65,13 +66,20 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface
const rclcpp_lifecycle::State & previous_state) override;

protected:
// For the PoseStamped message,
std::vector<std::string> joints_;
std::vector<std::string> buttons_;
std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;

Eigen::Matrix4d transform_, pose_;

// Publishers
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseStamped>> ee_pose_publisher_;
std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>
realtime_ee_pose_publisher_;
std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;

Eigen::Matrix4d transform_, pose_;
std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> fd_button_publisher_;
std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>
realtime_fd_button_publisher_;
};

} // namespace ee_pose_broadcaster
Expand Down
3 changes: 2 additions & 1 deletion fd_controllers/ee_pose_broadcaster/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,9 @@
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>rclcpp_lifecycle</depend>
<depend>geometry_msgs</depend>
<depend>realtime_tools</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>

<test_depend>ament_cmake_gmock</test_depend>
<test_depend>controller_manager</test_depend>
Expand Down
100 changes: 92 additions & 8 deletions fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ EePoseBroadcaster::on_init()
try {
// definition of the parameters that need to be queried from the
// controller configuration file with default values
auto_declare<std::vector<std::string>>("joints", std::vector<std::string>());
auto_declare<std::vector<std::string>>("buttons", std::vector<std::string>());
auto_declare<std::vector<double>>("transform_translation", std::vector<double>());
auto_declare<std::vector<double>>("transform_rotation", std::vector<double>());
} catch (const std::exception & e) {
Expand All @@ -76,13 +78,40 @@ EePoseBroadcaster::command_interface_configuration() const
controller_interface::InterfaceConfiguration EePoseBroadcaster::state_interface_configuration()
const
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::ALL};
controller_interface::InterfaceConfiguration state_interfaces_config;
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

if (joints_.empty()) {
RCLCPP_WARN(get_node()->get_logger(), "No joint name provided!");

} else {
for (const auto & joint : joints_) {
state_interfaces_config.names.push_back(joint + "/" + HW_IF_POSITION);
}
}

if (buttons_.empty()) {
RCLCPP_WARN(get_node()->get_logger(), "No button name provided!");

} else {
for (const auto & button : buttons_) {
state_interfaces_config.names.push_back(button + "/" + HW_IF_POSITION);
}
}

return state_interfaces_config;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
buttons_ = get_node()->get_parameter("buttons").as_string_array();
joints_ = get_node()->get_parameter("joints").as_string_array();
if (joints_.empty()) {
RCLCPP_ERROR(get_node()->get_logger(), "Please provide list of joints in config!");
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}

auto transform_trans_param = get_node()->get_parameter("transform_translation").as_double_array();
auto transform_rot_param = get_node()->get_parameter("transform_rotation").as_double_array();
Eigen::Quaternion<double> q;
Expand Down Expand Up @@ -119,7 +148,6 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state

std::cout << transform_ << std::endl;


try {
ee_pose_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
"ee_pose",
Expand All @@ -128,6 +156,13 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state
realtime_ee_pose_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::PoseStamped>>(
ee_pose_publisher_);

fd_button_publisher_ = get_node()->create_publisher<std_msgs::msg::Bool>(
"button_state", rclcpp::SystemDefaultsQoS());

realtime_fd_button_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(
fd_button_publisher_);
} catch (const std::exception & e) {
// get_node() may throw, logging raw here
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
Expand All @@ -139,12 +174,20 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
EePoseBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{
if (state_interfaces_.size() != (joints_.size())) {
RCLCPP_WARN(
get_node()->get_logger(),
"Not all requested interfaces exists.");
}

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
EePoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{
joints_.clear();

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

Expand All @@ -166,18 +209,50 @@ controller_interface::return_type EePoseBroadcaster::update(
const rclcpp::Duration & /*period*/)
{
for (const auto & state_interface : state_interfaces_) {
name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] =
name_if_value_mapping_[
state_interface.get_prefix_name()][state_interface.get_interface_name()] =
state_interface.get_value();
RCLCPP_DEBUG(
get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(),
get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_prefix_name().c_str(),
state_interface.get_interface_name().c_str(), state_interface.get_value());
}

if (realtime_ee_pose_publisher_ && realtime_ee_pose_publisher_->trylock()) {
pose_ = Eigen::Matrix4d::Identity();
pose_(0, 3) = get_value(name_if_value_mapping_, "fd_x", HW_IF_POSITION);
pose_(1, 3) = get_value(name_if_value_mapping_, "fd_y", HW_IF_POSITION);
pose_(2, 3) = get_value(name_if_value_mapping_, "fd_z", HW_IF_POSITION);

if (joints_.size() >= 3) {
double p_x = get_value(name_if_value_mapping_, joints_[0], HW_IF_POSITION);
double p_y = get_value(name_if_value_mapping_, joints_[1], HW_IF_POSITION);
double p_z = get_value(name_if_value_mapping_, joints_[2], HW_IF_POSITION);

if (std::isnan(p_x) || std::isnan(p_y) || std::isnan(p_z)) {
RCLCPP_DEBUG(
get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!");
return controller_interface::return_type::ERROR;
}
pose_(0, 3) = p_x;
pose_(1, 3) = p_y;
pose_(2, 3) = p_z;
}

if (joints_.size() >= 6) {
double roll = get_value(name_if_value_mapping_, joints_[3], HW_IF_POSITION);
double pitch = get_value(name_if_value_mapping_, joints_[4], HW_IF_POSITION);
double yaw = get_value(name_if_value_mapping_, joints_[5], HW_IF_POSITION);

if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) {
RCLCPP_DEBUG(
get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!");
return controller_interface::return_type::ERROR;
}

Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

Eigen::Quaterniond q = rollAngle * pitchAngle * yawAngle;
pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
}

pose_ = transform_ * pose_;
Eigen::Quaternion<double> q(pose_.block<3, 3>(0, 0));
Expand All @@ -198,6 +273,15 @@ controller_interface::return_type EePoseBroadcaster::update(
realtime_ee_pose_publisher_->unlockAndPublish();
}

if (!buttons_.empty()) {
if (realtime_fd_button_publisher_ && realtime_fd_button_publisher_->trylock()) {
auto & ee_button_msg = realtime_fd_button_publisher_->msg_;
double button_status = get_value(name_if_value_mapping_, buttons_[0], HW_IF_POSITION);
ee_button_msg.data = button_status > 0.5;
realtime_fd_button_publisher_->unlockAndPublish();
}
}

return controller_interface::return_type::OK;
}

Expand Down
34 changes: 29 additions & 5 deletions fd_description/config/fd_controllers.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@ fd/controller_manager:
fd_controller:
type: effort_controllers/JointGroupEffortController # ForwardCommandController

fd_ee_broadcaster:
type: ee_pose_broadcaster/EePoseBroadcaster

joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster

Expand All @@ -20,8 +23,29 @@ fd/fd_controller:
- fd_x
- fd_y
- fd_z
## Note: uncomment the following if using an omega 6/7 device
# - fd_roll
# - fd_pitch
# - fd_yaw
# - fd_clutch
## Note: uncomment the following if using an omega 7 device (e.g., 7 controlled DoF)
# - fd_roll
# - fd_pitch
# - fd_yaw
## Note: uncomment the following if a clutch is present (e.g., omega 6/7 device)
# - fd_clutch

fd/fd_ee_broadcaster:
ros__parameters:
transform_translation:
- 0.0
- 0.0
- 0.0
transform_rotation:
- 0.0
- 0.0
- 0.0
joints:
- fd_x
- fd_y
- fd_z
## Note: uncomment the following if orientation is enabled (e.g., omega 6/7 device)
# - fd_roll
# - fd_pitch
# - fd_yaw
buttons: [button] # Note: only one button supported for now
5 changes: 5 additions & 0 deletions fd_description/ros2_control/fd.r2c_hardware.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,11 @@
</xacro:unless>
</hardware>

<!-- Configure GPIOs -->
<gpio name="button">
<state_interface name="position"/>
</gpio>

<!-- Configure translations -->
<xacro:macro name="configure_translation_joint" params="joint_name">
<joint name="${joint_name}">
Expand Down
1 change: 1 addition & 0 deletions fd_hardware/include/fd_hardware/fd_effort_hi.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface
std::vector<double> hw_states_position_;
std::vector<double> hw_states_velocity_;
std::vector<double> hw_states_effort_;
std::vector<double> hw_button_state_;

/**
Initiate the USB communication with the device.
Expand Down
48 changes: 48 additions & 0 deletions fd_hardware/src/fd_effort_hi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ CallbackReturn FDEffortHardwareInterface::on_init(
hw_states_velocity_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_states_effort_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_commands_effort_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
hw_button_state_.resize(info_.gpios.size(), std::numeric_limits<double>::quiet_NaN());


for (const hardware_interface::ComponentInfo & joint : info_.joints) {
Expand Down Expand Up @@ -106,6 +107,38 @@ CallbackReturn FDEffortHardwareInterface::on_init(
return CallbackReturn::ERROR;
}
}
for (const hardware_interface::ComponentInfo & button : info_.gpios) {
if (button.state_interfaces.size() != 1) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
"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"),
"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;
}
}
for (const hardware_interface::ComponentInfo & button : info_.gpios) {
if (button.state_interfaces.size() != 1) {
RCLCPP_FATAL(
rclcpp::get_logger("FDEffortHardwareInterface"),
"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"),
"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;
}
}

return CallbackReturn::SUCCESS;
}
Expand All @@ -129,6 +162,11 @@ FDEffortHardwareInterface::export_state_interfaces()
hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i]));
}
for (uint i = 0; i < info_.gpios.size(); i++) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
info_.gpios[i].name, hardware_interface::HW_IF_POSITION, &hw_button_state_[i]));
}

return state_interfaces;
}
Expand Down Expand Up @@ -229,6 +267,16 @@ hardware_interface::return_type FDEffortHardwareInterface::read(
hw_states_effort_[6] = gripper_force;
}

// Get button status, TODO multiple buttons from index
int button_status = dhdGetButton(0, interface_ID_);
if (button_status == 1) {
hw_button_state_[0] = 1.0;
} else if (button_status == 0) {
hw_button_state_[0] = 0.0;
} else {
flag += -1;
}

if (flag >= 0) {
return hardware_interface::return_type::OK;
} else {
Expand Down

0 comments on commit 14ad4ca

Please sign in to comment.