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

Revert "Rollback to original (diagnostics is now in separate branch)"

Browse Source 
This reverts commit 644ea2491c.
This commit is contained in:
Simon
2015-11-12 13:45:07 +01:00
parent c18ff438ca
commit 777cf2fe71
1 changed files with 63 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
src/ur_ros_wrapper.cpp
View File

@@ -39,6 +39,37 @@
#include "ur_msgs/Analog.h"
#include "std_msgs/String.h"
#include <controller_manager/controller_manager.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
enum RobotMode {
ROBOT_MODE_DISCONNECTED,
ROBOT_MODE_CONFIRM_SAFETY,
ROBOT_MODE_BOOTING,
ROBOT_MODE_POWER_OFF,
ROBOT_MODE_POWER_ON,
ROBOT_MODE_IDLE,
ROBOT_MODE_BACKDRIVE,
ROBOT_MODE_RUNNING,
ROBOT_MODE_UPDATING_FIRMWARE
};
enum SafetyMode {
SAFETY_MODE_NORMAL,
SAFETY_MODE_REDUCED,
SAFETY_MODE_PROTECTIVE_STOP,
SAFETY_MODE_RECOVERY,
SAFETY_MODE_SAFEGUARD_STOP,
SAFETY_MODE_SYSTEM_EMERGENCY_STOP,
SAFETY_MODE_ROBOT_EMERGENCY_STOP,
SAFETY_MODE_VIOLATION,
SAFETY_MODE_FAULT
};
struct DriverStatus{
RobotMode robot_mode;
SafetyMode safety_mode;
};
class RosWrapper {
protected:
@@ -54,8 +85,8 @@ protected:
ros::Subscriber urscript_sub_;
ros::ServiceServer io_srv_;
ros::ServiceServer payload_srv_;
std::thread* rt_publish_thread_;
std::thread* mb_publish_thread_;
std::thread* rt_publish_thread_;
std::thread* mb_publish_thread_;
double io_flag_delay_;
double max_velocity_;
std::vector<double> joint_offsets_;
@@ -63,6 +94,8 @@ protected:
std::thread* ros_control_thread_;
boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
diagnostic_updater::Updater updater_;
DriverStatus driver_status_;
public:
RosWrapper(std::string host) :
@@ -74,6 +107,9 @@ public:
std::vector<std::string> joint_names;
char buf[256];
updater_.setHardwareID("ur_driver");
updater_.add("Status updater", this, &RosWrapper::driverDiagnostic);
if (ros::param::get("~prefix", joint_prefix)) {
sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
print_info(buf);
@@ -170,6 +206,26 @@ public:
}
private:
void driverDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat){
if (driver_status_.robot_mode == ROBOT_MODE_RUNNING &&
(driver_status_.safety_mode == SAFETY_MODE_NORMAL || driver_status_.safety_mode == SAFETY_MODE_REDUCED)){
//Robot is running and in normal safety mode
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Driver OK");
}
else if (driver_status_.robot_mode == ROBOT_MODE_RUNNING &&
(driver_status_.safety_mode != SAFETY_MODE_NORMAL || driver_status_.safety_mode != SAFETY_MODE_REDUCED)){
//Robot is running and in some safety stop
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Driver WARNING");
}
else {
//Robot is not running
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver ERROR");
}
stat.add("Robot mode", driver_status_.robot_mode);
stat.add("Safety mode", driver_status_.safety_mode);
}
void goalCB() {
print_info("on_goal");
@@ -453,6 +509,11 @@ private:
wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg);
//Update diagnostics
driver_status_.robot_mode = static_cast<RobotMode>( robot_.rt_interface_->robot_state_->getRobotMode() );
driver_status_.safety_mode = static_cast<SafetyMode>( robot_.rt_interface_->robot_state_->getSafety_mode() );
updater_.update();
robot_.rt_interface_->robot_state_->setDataPublished();
}