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

moved reading of rtde values into separate function

Browse Source
This commit is contained in:
Felix Mauch
2019-07-18 11:46:20 +02:00
parent e63de16225
commit 9a2d890c6d
2 changed files with 22 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -97,6 +97,9 @@ protected:
bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
template <typename T>
void readData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg, const std::string& var_name, T& data);
std::unique_ptr<UrDriver> ur_driver_; std::unique_ptr<UrDriver> ur_driver_;
ros::ServiceServer deactivate_srv_; ros::ServiceServer deactivate_srv_;

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

@@ -193,46 +193,30 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
return true; return true;
} }
template <typename T>
void HardwareInterface::readData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg,
const std::string& var_name, T& data)
{
if (!data_pkg->getData(var_name, data))
{
// This throwing should never happen unless misconfigured
std::string error_msg = "Did not find '" + var_name + "' in data sent from robot. This should not happen!";
throw std::runtime_error(error_msg);
}
}
void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period) void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period)
{ {
std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage(); std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
if (data_pkg) if (data_pkg)
{ {
if (!data_pkg->getData("actual_q", joint_positions_)) readData(data_pkg, "actual_q", joint_positions_);
{ readData(data_pkg, "actual_qd", joint_velocities_);
// This throwing should never happen unless misconfigured readData(data_pkg, "target_speed_fraction", target_speed_fraction_);
throw std::runtime_error("Did not find joint position in data sent from robot. This should not happen!"); readData(data_pkg, "speed_scaling", speed_scaling_);
} readData(data_pkg, "runtime_state", runtime_state_);
if (!data_pkg->getData("actual_qd", joint_velocities_)) readData(data_pkg, "actual_TCP_force", fts_measurements_);
{ readData(data_pkg, "actual_TCP_pose", tcp_pose_);
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find joint velocity in data sent from robot. This should not happen!");
}
if (!data_pkg->getData("target_speed_fraction", target_speed_fraction_))
{
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find target_speed_fraction_ in data sent from robot. This should not happen!");
}
if (!data_pkg->getData("speed_scaling", speed_scaling_))
{
// 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("runtime_state", runtime_state_))
{
// 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!");
}
if (!data_pkg->getData("actual_TCP_pose", tcp_pose_))
{
// This throwing should never happen unless misconfigured
throw std::runtime_error("Did not find actual_TCP_pose in data sent from robot. This should not happen!");
}
// Transform fts measurements to tool frame // Transform fts measurements to tool frame
extractToolPose(time); extractToolPose(time);