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

Publish actual tool pose as pose msg and tf

Browse Source
This commit is contained in:
Simon
2015-11-12 17:22:20 +01:00
parent 634c8eae53
commit 4cdceac791
3 changed files with 41 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
trajectory_msgs
ur_msgs
tf
)
## System dependencies are found with CMake's conventions

2
package.xml
View File

@@ -50,6 +50,7 @@
<build_depend>std_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>ur_msgs</build_depend>
<build_depend>tf</build_depend>
<run_depend>hardware_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>ros_controllers</run_depend>
@@ -62,6 +63,7 @@
<run_depend>trajectory_msgs</run_depend>
<run_depend>ur_msgs</run_depend>
<run_depend>ur_description</run_depend>
<run_depend>tf</run_depend>
<!-- The export tag contains other, unspecified, tags -->

38
src/ur_ros_wrapper.cpp
View File

@@ -33,6 +33,7 @@
#include <ros/console.h>
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/WrenchStamped.h"
#include "geometry_msgs/PoseStamped.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
#include "actionlib/server/action_server.h"
#include "actionlib/server/server_goal_handle.h"
@@ -49,6 +50,10 @@
#include "std_msgs/String.h"
#include <controller_manager/controller_manager.h>
/// TF
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
class RosWrapper {
protected:
UrDriver robot_;
@@ -541,10 +546,13 @@ private:
"joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
"wrench", 1);
ros::Publisher tool_pose_pub = nh_.advertise<geometry_msgs::PoseStamped>("tool_pose", 1);
static tf::TransformBroadcaster br;
while (ros::ok()) {
sensor_msgs::JointState joint_msg;
joint_msg.name = robot_.getJointNames();
geometry_msgs::WrenchStamped wrench_msg;
geometry_msgs::PoseStamped tool_pose_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getDataPublished()) {
@@ -571,6 +579,36 @@ private:
wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg);
// Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
tool_pose_msg.pose.position.x = tool_vector_actual[0];
tool_pose_msg.pose.position.y = tool_vector_actual[1];
tool_pose_msg.pose.position.z = tool_vector_actual[2];
tf::Quaternion quat;
// quat.setEuler(tool_vector_actual[4], tool_vector_actual[3], tool_vector_actual[5]);
double rx = tool_vector_actual[3];
double ry = tool_vector_actual[4];
double rz = tool_vector_actual[5];
double angle = std::sqrt(rx*rx + ry*ry + rz*rz);
rx = rx/angle;
ry = ry/angle;
rz = rz/angle;
quat.setRotation(tf::Vector3(rx, ry, rz), angle);
// quat.setRotation(tf::Vector3(1,0,0), tool_vector_actual[3]);
// quat.setRotation(tf::Vector3(0,1,0), tool_vector_actual[4]);
// quat.setRotation(tf::Vector3(0,0,1), tool_vector_actual[5]);
// quat.setRPY(tool_vector_actual[3], tool_vector_actual[4], tool_vector_actual[5]);
tf::quaternionTFToMsg(quat, tool_pose_msg.pose.orientation);
tool_pose_pub.publish(tool_pose_msg);
tf::Transform transform;
transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2]));
transform.setRotation(quat);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "philips/ur_base_link", "philips/ur_ee_link_calibrated"));
robot_.rt_interface_->robot_state_->setDataPublished();
}