diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index 9651459..7b0b0be 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -70,12 +70,14 @@ protected: hardware_interface::PositionJointInterface pj_interface_; ur_controllers::SpeedScalingInterface speedsc_interface_; // hardware_interface::VelocityJointInterface vj_interface_; + hardware_interface::ForceTorqueSensorInterface fts_interface_; vector6d_t joint_position_command_; // std::vector joint_velocity_command_; vector6d_t joint_positions_; vector6d_t joint_velocities_; vector6d_t joint_efforts_; + vector6d_t fts_measurements_; double speed_scaling_; double target_speed_fraction_; double speed_scaling_combined_; @@ -86,6 +88,8 @@ protected: PausingState pausing_state_; double pausing_ramp_up_increment_; + + std::string tcp_link_; }; } // namespace ur_driver diff --git a/ur_rtde_driver/launch/ur10_ros_control.launch b/ur_rtde_driver/launch/ur10_ros_control.launch index 506b5cc..2ee6c82 100644 --- a/ur_rtde_driver/launch/ur10_ros_control.launch +++ b/ur_rtde_driver/launch/ur10_ros_control.launch @@ -16,7 +16,7 @@ - + diff --git a/ur_rtde_driver/resources/rtde_recipe.txt b/ur_rtde_driver/resources/rtde_recipe.txt index e4ef814..e063431 100644 --- a/ur_rtde_driver/resources/rtde_recipe.txt +++ b/ur_rtde_driver/resources/rtde_recipe.txt @@ -4,3 +4,4 @@ actual_qd speed_scaling target_speed_fraction runtime_state +actual_TCP_force diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 78c8584..256ecf9 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -61,6 +61,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h return false; } + tcp_link_ = robot_hw_nh.param("tcp_link", "tool0"); + ROS_INFO_STREAM("Initializing urdriver"); ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename)); @@ -90,11 +92,15 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h speedsc_interface_.registerHandle( ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_)); + fts_interface_.registerHandle(hardware_interface::ForceTorqueSensorHandle( + "wrench", tcp_link_, fts_measurements_.begin(), fts_measurements_.begin() + 3)); + // Register interfaces registerInterface(&js_interface_); registerInterface(&spj_interface_); // registerInterface(&pj_interface_); registerInterface(&speedsc_interface_); + registerInterface(&fts_interface_); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface"); @@ -131,6 +137,11 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period // This throwing should never happen unless misconfigured throw std::runtime_error("Did not find speed_scaling in data sent from robot. This should not happen!"); } + if (!data_pkg->getData("actual_TCP_force", fts_measurements_)) + { + // This throwing should never happen unless misconfigured + throw std::runtime_error("Did not find actual_TCP_force in data sent from robot. This should not happen!"); + } // pausing state follows runtime state when pausing if (runtime_state_ == static_cast(rtde_interface::RUNTIME_STATE::PAUSED))