diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e273943..44518e22 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS actionlib control_msgs geometry_msgs + industrial_msgs roscpp sensor_msgs std_srvs diff --git a/include/ur_modern_driver/ros/mb_publisher.h b/include/ur_modern_driver/ros/mb_publisher.h index 692ecfd6..f4b15496 100644 --- a/include/ur_modern_driver/ros/mb_publisher.h +++ b/include/ur_modern_driver/ros/mb_publisher.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -14,6 +15,7 @@ class MBPublisher : public URStatePacketConsumer private: NodeHandle nh_; Publisher io_pub_; + Publisher status_pub_; template inline void appendDigital(std::vector& vec, std::bitset bits) @@ -28,9 +30,13 @@ class MBPublisher : public URStatePacketConsumer } void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data); + void publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const; + void publishRobotStatus(const RobotModeData_V1_X& data) const; + void publishRobotStatus(const RobotModeData_V3_0__1& data) const; public: MBPublisher() : io_pub_(nh_.advertise("ur_driver/io_states", 1)) + , status_pub_(nh_.advertise("ur_driver/robot_status", 1)) { } @@ -51,4 +57,4 @@ class MBPublisher : public URStatePacketConsumer virtual void stopConsumer() { } -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h index f4945d9a..ce9b320a 100644 --- a/include/ur_modern_driver/ros/rt_publisher.h +++ b/include/ur_modern_driver/ros/rt_publisher.h @@ -71,4 +71,4 @@ class RTPublisher : public URRTPacketConsumer virtual void stopConsumer() { } -}; \ No newline at end of file +}; diff --git a/package.xml b/package.xml index e451bc36..7e2ac0c5 100644 --- a/package.xml +++ b/package.xml @@ -45,6 +45,7 @@ actionlib control_msgs geometry_msgs + industrial_msgs roscpp sensor_msgs trajectory_msgs @@ -57,6 +58,7 @@ actionlib control_msgs geometry_msgs + industrial_msgs roscpp sensor_msgs trajectory_msgs diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp index c0fdc232..9d9832f0 100644 --- a/src/ros/mb_publisher.cpp +++ b/src/ros/mb_publisher.cpp @@ -18,6 +18,63 @@ void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data io_pub_.publish(io_msg); } +void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const +{ + //note that this is true as soon as the drives are powered, + //even if the breakes are still closed + //which is in slight contrast to the comments in the + //message definition + status.drives_powered.val = data.robot_power_on; + + status.e_stopped.val = data.emergency_stopped; + + //I found no way to reliably get information if the robot is moving + //data.programm_running would be true when using this driver to move the robot + //but it would also be true when another programm is running that might not + //in fact contain any movement commands + status.in_motion.val = industrial_msgs::TriState::UNKNOWN; + + //the error code, if any, is not transmitted by this protocol + //it can and should be fetched seperately + status.error_code = 0; + + //note that e-stop is handled by a seperate variable + status.in_error.val = data.protective_stopped; + + status_pub_.publish(status); +} + +void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const +{ + industrial_msgs::RobotStatus msg; + + if (data.robot_mode == robot_mode_V1_X::ROBOT_FREEDRIVE_MODE) + msg.mode.val = industrial_msgs::RobotMode::MANUAL; + else + msg.mode.val = industrial_msgs::RobotMode::AUTO; + + //todo: verify that this correct, there is also ROBOT_READY_MODE + msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) + ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF; + + publishRobotStatus(msg, data); +} + +void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const +{ + industrial_msgs::RobotStatus msg; + + msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING) + ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF; + + if (data.control_mode == robot_control_mode_V3_X::TEACH) + msg.mode.val = industrial_msgs::RobotMode::MANUAL; + else + msg.mode.val = industrial_msgs::RobotMode::AUTO; + + publishRobotStatus(msg, data); +} + bool MBPublisher::consume(MasterBoardData_V1_X& data) { ur_msgs::IOStates io_msg; @@ -42,13 +99,16 @@ bool MBPublisher::consume(MasterBoardData_V3_2& data) bool MBPublisher::consume(RobotModeData_V1_X& data) { + publishRobotStatus(data); return true; } bool MBPublisher::consume(RobotModeData_V3_0__1& data) { + publishRobotStatus(data); return true; } bool MBPublisher::consume(RobotModeData_V3_2& data) { + publishRobotStatus(data); return true; -} \ No newline at end of file +} diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp index 65d07266..0709f506 100644 --- a/src/ros/rt_publisher.cpp +++ b/src/ros/rt_publisher.cpp @@ -116,4 +116,4 @@ bool RTPublisher::consume(RTState_V3_0__1& state) bool RTPublisher::consume(RTState_V3_2__3& state) { return publish(state); -} \ No newline at end of file +}