Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

added a publisher for the tcp pose

Browse Source
This commit is contained in:
Felix Mauch
2019-06-13 13:47:56 +02:00
committed by Tristan Schnell
parent c3d92dcde5
commit c714af16ff
2 changed files with 28 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
View File

@@ -33,6 +33,7 @@
#include <hardware_interface/joint_state_interface.h> #include <hardware_interface/joint_state_interface.h>
#include <algorithm> #include <algorithm>
#include <std_msgs/Bool.h> #include <std_msgs/Bool.h>
#include <realtime_tools/realtime_publisher.h>
#include "tf2_msgs/TFMessage.h" #include "tf2_msgs/TFMessage.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -85,6 +86,14 @@ protected:
* \param timestamp Timestamp of read data * \param timestamp Timestamp of read data
*/ */
void extractToolPose(const ros::Time& timestamp); void extractToolPose(const ros::Time& timestamp);
/*!
* \brief Publishes the tool pose to the tf system
*
* Requires extractToolPose() to be run first.
*/
void publishPose();
std::unique_ptr<UrDriver> ur_driver_; std::unique_ptr<UrDriver> ur_driver_;
hardware_interface::JointStateInterface js_interface_; hardware_interface::JointStateInterface js_interface_;
@@ -109,6 +118,8 @@ protected:
double speed_scaling_combined_; double speed_scaling_combined_;
std::vector<std::string> joint_names_; std::vector<std::string> joint_names_;
std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> tcp_pose_pub_;
uint32_t runtime_state_; uint32_t runtime_state_;
bool position_controller_running_; bool position_controller_running_;

17
ur_rtde_driver/src/ros/hardware_interface.cpp
View File

@@ -181,6 +181,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
registerInterface(&speedsc_interface_); registerInterface(&speedsc_interface_);
registerInterface(&fts_interface_); registerInterface(&fts_interface_);
tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface"); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
return true; return true;
@@ -230,6 +232,7 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
// Transform fts measurements to tool frame // Transform fts measurements to tool frame
extractToolPose(time); extractToolPose(time);
transformForceTorque(); transformForceTorque();
publishPose();
// pausing state follows runtime state when pausing // pausing state follows runtime state when pausing
if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED)) if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED))
@@ -376,4 +379,18 @@ void HardwareInterface ::extractToolPose(const ros::Time& timestamp)
tcp_transform_.transform.rotation = tf2::toMsg(rotation); tcp_transform_.transform.rotation = tf2::toMsg(rotation);
} }
void HardwareInterface ::publishPose()
{
if (tcp_pose_pub_)
{
if (tcp_pose_pub_->trylock())
{
tcp_pose_pub_->msg_.transforms.clear();
tcp_pose_pub_->msg_.transforms.push_back(tcp_transform_);
tcp_pose_pub_->unlockAndPublish();
}
}
}
} // namespace ur_driver } // namespace ur_driver