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

Adopted roscpp code style and naming convention

Browse Source
This commit is contained in:
Simon Rasmussen
2017-03-01 13:59:48 +01:00
parent e4bc40fc09
commit 474f469e97
44 changed files with 5097 additions and 4891 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
src/ros/rt_publisher.cpp
View File

@@ -1,119 +1,122 @@
#include "ur_modern_driver/ros/rt_publisher.h"
bool RTPublisher::publish_joints(RTShared& packet, Time& t)
bool RTPublisher::publishJoints(RTShared& packet, Time& t)
{
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name = _joint_names;
sensor_msgs::JointState joint_msg;
joint_msg.header.stamp = t;
joint_msg.name = joint_names_;
for (auto const& q : packet.q_actual) {
joint_msg.position.push_back(q);
}
for (auto const& qd : packet.qd_actual) {
joint_msg.velocity.push_back(qd);
}
for (auto const& i : packet.i_actual) {
joint_msg.effort.push_back(i);
}
for (auto const& q : packet.q_actual)
{
joint_msg.position.push_back(q);
}
for (auto const& qd : packet.qd_actual)
{
joint_msg.velocity.push_back(qd);
}
for (auto const& i : packet.i_actual)
{
joint_msg.effort.push_back(i);
}
_joint_pub.publish(joint_msg);
joint_pub_.publish(joint_msg);
return true;
return true;
}
bool RTPublisher::publish_wrench(RTShared& packet, Time& t)
bool RTPublisher::publishWrench(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];
wrench_msg.wrench.torque.x = packet.tcp_force[3];
wrench_msg.wrench.torque.y = packet.tcp_force[4];
wrench_msg.wrench.torque.z = packet.tcp_force[5];
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];
wrench_msg.wrench.torque.x = packet.tcp_force[3];
wrench_msg.wrench.torque.y = packet.tcp_force[4];
wrench_msg.wrench.torque.z = packet.tcp_force[5];
_wrench_pub.publish(wrench_msg);
return true;
wrench_pub_.publish(wrench_msg);
return true;
}
bool RTPublisher::publish_tool(RTShared& packet, Time& t)
bool RTPublisher::publishTool(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;
tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
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;
tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x;
tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y;
tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z;
_tool_vel_pub.publish(tool_twist);
return true;
tool_vel_pub_.publish(tool_twist);
return true;
}
bool RTPublisher::publish_transform(RTShared& packet, Time& t)
bool RTPublisher::publishTransform(RTShared& packet, Time& t)
{
auto tv = packet.tool_vector_actual;
auto tv = packet.tool_vector_actual;
Transform transform;
transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
Transform transform;
transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z));
// Create quaternion
Quaternion quat;
//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);
}
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.setRotation(quat);
transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_));
_transform_broadcaster.sendTransform(StampedTransform(transform, t, _base_frame, _tool_frame));
return true;
return true;
}
bool RTPublisher::publish_temperature(RTShared& packet, Time& t)
bool RTPublisher::publishTemperature(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];
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;
joint_temperature_pub_.publish(msg);
}
return true;
}
bool RTPublisher::publish(RTShared& packet)
{
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);
Time time = Time::now();
return publishJoints(packet, time) && publishWrench(packet, time) && publishTool(packet, time) &&
publishTransform(packet, time) && publishTemperature(packet, time);
}
bool RTPublisher::consume(RTState_V1_6__7& state)
{
return publish(state);
return publish(state);
}
bool RTPublisher::consume(RTState_V1_8& state)
{
return publish(state);
return publish(state);
}
bool RTPublisher::consume(RTState_V3_0__1& state)
{
return publish(state);
return publish(state);
}
bool RTPublisher::consume(RTState_V3_2__3& state)
{
return publish(state);
return publish(state);
}