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

Rollback to original (diagnostics is now in separate branch)

Browse Source
This commit is contained in:
Simon
2015-10-09 16:44:00 +02:00
parent 5c7daf4365
commit 644ea2491c
1 changed files with 2 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
src/ur_ros_wrapper.cpp
View File

@@ -39,37 +39,6 @@
#include "ur_msgs/Analog.h" #include "ur_msgs/Analog.h"
#include "std_msgs/String.h" #include "std_msgs/String.h"
#include <controller_manager/controller_manager.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 { class RosWrapper {
protected: protected:
@@ -85,8 +54,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_;
@@ -94,8 +63,6 @@ protected:
std::thread* ros_control_thread_; std::thread* ros_control_thread_;
boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_; boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
boost::shared_ptr<controller_manager::ControllerManager> controller_manager_; boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
diagnostic_updater::Updater updater_;
DriverStatus driver_status_;
public: public:
RosWrapper(std::string host) : RosWrapper(std::string host) :
@@ -107,9 +74,6 @@ public:
std::vector<std::string> joint_names; std::vector<std::string> joint_names;
char buf[256]; char buf[256];
updater_.setHardwareID("ur_driver");
updater_.add("Status updater", this, &RosWrapper::driverDiagnostic);
if (ros::param::get("~prefix", joint_prefix)) { if (ros::param::get("~prefix", joint_prefix)) {
sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
print_info(buf); print_info(buf);
@@ -206,26 +170,6 @@ public:
} }
private: 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() { void goalCB() {
print_info("on_goal"); print_info("on_goal");
@@ -509,11 +453,6 @@ 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);
//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(); robot_.rt_interface_->robot_state_->setDataPublished();
} }