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

correctly shutdown comm threads on exit

Browse Source
This commit is contained in:
Felix Mauch
2019-05-09 16:04:21 +02:00
parent c79c567412
commit 9ac939b2b0
3 changed files with 38 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h
View File

@@ -152,6 +152,11 @@ public:
{ {
} }
virtual ~Pipeline()
{
stop();
}
void run() void run()
{ {
if (running_) if (running_)
@@ -170,7 +175,7 @@ public:
if (!running_) if (!running_)
return; return;
LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); LOG_INFO("Stopping pipeline! <%s>", name_.c_str());
if (consumer_ != nullptr) if (consumer_ != nullptr)
consumer_->stopConsumer(); consumer_->stopConsumer();

5
ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h
View File

@@ -52,7 +52,10 @@ public:
throw std::runtime_error("Failed to accept robot connection"); throw std::runtime_error("Failed to accept robot connection");
} }
} }
~ReverseInterface() = default; ~ReverseInterface()
{
server_.disconnectClient();
}
bool write(const vector6d_t& positions) bool write(const vector6d_t& positions)
{ {

41
ur_rtde_driver/src/ros/hardware_interface_node.cpp
View File

@@ -27,8 +27,22 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <controller_manager/controller_manager.h> #include <controller_manager/controller_manager.h>
#include <csignal>
#include <ur_rtde_driver/ros/hardware_interface.h> #include <ur_rtde_driver/ros/hardware_interface.h>
std::unique_ptr<ur_driver::HardwareInterface> hw_interface;
void signalHandler(int signum)
{
std::cout << "Interrupt signal (" << signum << ") received.\n";
hw_interface.reset();
// cleanup and close up stuff here
// terminate program
exit(signum);
}
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
// Set up ROS. // Set up ROS.
@@ -39,7 +53,8 @@ int main(int argc, char** argv)
ros::NodeHandle nh; ros::NodeHandle nh;
ros::NodeHandle nh_priv("~"); ros::NodeHandle nh_priv("~");
ur_driver::HardwareInterface hw_interface; // register signal SIGINT and signal handler
signal(SIGINT, signalHandler);
// Set up timers // Set up timers
ros::Time timestamp; ros::Time timestamp;
@@ -47,9 +62,11 @@ int main(int argc, char** argv)
auto stopwatch_last = std::chrono::steady_clock::now(); auto stopwatch_last = std::chrono::steady_clock::now();
auto stopwatch_now = stopwatch_last; auto stopwatch_now = stopwatch_last;
hw_interface.init(nh, nh_priv); hw_interface.reset(new ur_driver::HardwareInterface);
hw_interface->init(nh, nh_priv);
ROS_INFO_STREAM("initialized hw interface"); ROS_INFO_STREAM("initialized hw interface");
controller_manager::ControllerManager cm(&hw_interface, nh); controller_manager::ControllerManager cm(hw_interface.get(), nh);
ROS_INFO_STREAM("started controller manager"); ROS_INFO_STREAM("started controller manager");
// Get current time and elapsed time since last read // Get current time and elapsed time since last read
@@ -58,8 +75,6 @@ int main(int argc, char** argv)
period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count()); period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now; stopwatch_last = stopwatch_now;
ros::Rate control_rate(static_cast<double>(hw_interface.getControlFrequency()));
std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
bool has_realtime; bool has_realtime;
realtime_file >> has_realtime; realtime_file >> has_realtime;
@@ -109,11 +124,13 @@ int main(int argc, char** argv)
} }
} }
double expected_cycle_time = 1.0 / (static_cast<double>(hw_interface->getControlFrequency()));
// Run as fast as possible // Run as fast as possible
while (ros::ok()) while (ros::ok())
{ {
// Receive current state from robot // Receive current state from robot
hw_interface.read(timestamp, period); hw_interface->read(timestamp, period);
// Get current time and elapsed time since last read // Get current time and elapsed time since last read
timestamp = ros::Time::now(); timestamp = ros::Time::now();
@@ -123,14 +140,12 @@ int main(int argc, char** argv)
cm.update(timestamp, period); cm.update(timestamp, period);
hw_interface.write(timestamp, period); hw_interface->write(timestamp, period);
// if (!control_rate.sleep())
if (!control_rate.sleep()) if (period.toSec() > expected_cycle_time)
{ {
ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << // ROS_WARN_STREAM("Could not keep cycle rate of " << expected_cycle_time * 1000 << "ms");
"ms"); // ROS_WARN_STREAM("Actual cycle time:" << period.toNSec() / 1000000.0 << "ms");
ROS_WARN_STREAM("Actual cycle time:" << control_rate.cycleTime().toNSec() / 1000000.0 <<
"ms");
} }
} }