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

Publish robot_state

Browse Source
This commit is contained in:
Simon
2015-10-08 15:06:04 +02:00
parent f592d2189a
commit 7fdff1c66d
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/ur_ros_wrapper.cpp
View File

@@ -38,6 +38,7 @@
#include "ur_msgs/Digital.h" #include "ur_msgs/Digital.h"
#include "ur_msgs/Analog.h" #include "ur_msgs/Analog.h"
#include "std_msgs/String.h" #include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include <controller_manager/controller_manager.h> #include <controller_manager/controller_manager.h>
class RosWrapper { class RosWrapper {
@@ -54,8 +55,8 @@ protected:
ros::Subscriber urscript_sub_; ros::Subscriber urscript_sub_;
ros::ServiceServer io_srv_; ros::ServiceServer io_srv_;
ros::ServiceServer payload_srv_; ros::ServiceServer payload_srv_;
std::thread* rt_publish_thread_; std::thread* rt_publish_thread_;
std::thread* mb_publish_thread_; std::thread* mb_publish_thread_;
double io_flag_delay_; double io_flag_delay_;
double max_velocity_; double max_velocity_;
std::vector<double> joint_offsets_; std::vector<double> joint_offsets_;
@@ -423,6 +424,8 @@ private:
"joint_states", 1); "joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>( ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
"wrench", 1); "wrench", 1);
ros::Publisher robot_state_pub = nh_.advertise<std_msgs::Int32>(
"ur_driver/robot_state", 1);
while (ros::ok()) { while (ros::ok()) {
sensor_msgs::JointState joint_msg; sensor_msgs::JointState joint_msg;
joint_msg.name = robot_.getJointNames(); joint_msg.name = robot_.getJointNames();
@@ -453,6 +456,11 @@ private:
wrench_msg.wrench.torque.z = tcp_force[5]; wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg); wrench_pub.publish(wrench_msg);
//Publish robot state
std_msgs::Int32 robot_state_msg;
robot_state_msg.data = (int) robot_.rt_interface_->robot_state_->getRobotMode();
robot_state_pub.publish(robot_state_msg);
robot_.rt_interface_->robot_state_->setDataPublished(); robot_.rt_interface_->robot_state_->setDataPublished();
} }