diff --git a/fd_bringup/launch/fd.launch.py b/fd_bringup/launch/fd.launch.py index 1a14bef..29ec889 100644 --- a/fd_bringup/launch/fd.launch.py +++ b/fd_bringup/launch/fd.launch.py @@ -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', diff --git a/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp b/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp index 95537c5..b88d550 100644 --- a/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp +++ b/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp @@ -69,6 +69,7 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface std::shared_ptr> ee_pose_publisher_; std::shared_ptr> realtime_ee_pose_publisher_; + std::vector joints_; std::unordered_map> name_if_value_mapping_; Eigen::Matrix4d transform_, pose_; diff --git a/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp b/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp index 3eb36bd..5730014 100644 --- a/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp +++ b/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp @@ -56,6 +56,7 @@ EePoseBroadcaster::on_init() try { // definition of the parameters that need to be queried from the // controller configuration file with default values + auto_declare>("joints", std::vector()); auto_declare>("transform_translation", std::vector()); auto_declare>("transform_rotation", std::vector()); } catch (const std::exception & e) { @@ -76,13 +77,30 @@ 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); + } + } + + return state_interfaces_config; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + 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 q; @@ -139,12 +157,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; } @@ -166,18 +192,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 q(pose_.block<3, 3>(0, 0)); diff --git a/fd_description/config/fd_controllers.yaml b/fd_description/config/fd_controllers.yaml index e50facd..14ca6c4 100644 --- a/fd_description/config/fd_controllers.yaml +++ b/fd_description/config/fd_controllers.yaml @@ -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 @@ -20,8 +23,28 @@ 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