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

Improved RTPublisher

Browse Source
This commit is contained in:
Simon Rasmussen
2017-02-16 22:49:10 +01:00
parent a78d3eadf3
commit b422107c08
2 changed files with 62 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
src/ros/rt_publisher.cpp
View File

@@ -1,6 +1,6 @@
#include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
bool RTPublisher::publish_joints(RTShared& packet, Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
@@ -21,11 +21,10 @@ bool RTPublisher::publish_joints(RTShared& packet, ros::Time& t)
return true;
}
bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t)
bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
{
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = t;
wrench_msg.wrench.force.x = packet.tcp_force[0];
wrench_msg.wrench.force.y = packet.tcp_force[1];
wrench_msg.wrench.force.z = packet.tcp_force[2];
@@ -34,18 +33,14 @@ bool RTPublisher::publish_wrench(RTShared& packet, ros::Time& t)
wrench_msg.wrench.torque.z = packet.tcp_force[5];
_wrench_pub.publish(wrench_msg);
return true;
}
bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t)
bool RTPublisher::publish_tool(RTShared& packet, Time& t)
{
geometry_msgs::TwistStamped tool_twist;
tool_twist.header.stamp = t;
tool_twist.header.frame_id = _base_frame;
tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x;
tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y;
tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z;
@@ -54,14 +49,56 @@ bool RTPublisher::publish_tool(RTShared& packet, ros::Time& t)
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
_tool_vel_pub.publish(tool_twist);
return true;
}
bool RTPublisher::publish_transform(RTShared& packet, Time& t)
{
auto tv = packet.tool_vector_actual;
Transform transform;
transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
//Create quaternion
Quaternion quat;
double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2));
if (angle < 1e-16) {
quat.setValue(0, 0, 0, 1);
} else {
quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle);
}
transform.setRotation(quat);
_transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame));
return true;
}
bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
{
size_t len = _joint_names.size();
for (size_t i = 0; i < len; i++) {
sensor_msgs::Temperature msg;
msg.header.stamp = t;
msg.header.frame_id = _joint_names[i];
msg.temperature = packet.motor_temperatures[i];
_joint_temperature_pub.publish(msg);
}
return true;
}
bool RTPublisher::publish(RTShared& packet)
{
ros::Time time = ros::Time::now();
return publish_joints(packet, time) && publish_wrench(packet, time) && publish_tool(packet, time);
Time time = Time::now();
return publish_joints(packet, time)
&& publish_wrench(packet, time)
&& publish_tool(packet, time)
&& publish_transform(packet, time)
&& publish_temperature(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7& state)