Skip to content

Commit

Permalink
Add button as gpio (#15)
Browse files Browse the repository at this point in the history
* Add button as gpio:
  - gpio for HW
  - add publisher in ee_pose_brd
  - !!! only one button supported !!!

Note: refractored version of CarlDegio/forcedimension_ros2 (see PR #4).

Co-authored-by: CarlDegio <2536890272@qq.com>

* use "std_msgs/msg/bool"

* fix CmakeLists

* clean-up config

---------

Co-authored-by: CarlDegio <2536890272@qq.com>
  • Loading branch information
tpoignonec and CarlDegio authored Jan 8, 2024
1 parent 561131e commit cc387e8
Show file tree
Hide file tree
Showing 8 changed files with 98 additions and 6 deletions.
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,14 +66,20 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface
const rclcpp_lifecycle::State & previous_state) override;

protected:
// For the PoseStamped message,
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::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::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
28 changes: 27 additions & 1 deletion fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ EePoseBroadcaster::on_init()
// 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 Down Expand Up @@ -89,12 +90,22 @@ const
}
}

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!");
Expand Down Expand Up @@ -137,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 @@ -146,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 Down Expand Up @@ -256,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
1 change: 1 addition & 0 deletions fd_description/config/fd_controllers.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -48,3 +48,4 @@ fd/fd_ee_broadcaster:
# - 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 @@ -67,6 +67,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 @@ -48,6 +48,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 @@ -98,6 +99,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 @@ -121,6 +154,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 @@ -221,6 +259,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 cc387e8

Please sign in to comment.