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

Re-add urscript topic (#7)

Browse Source 
* Re-added UR script - for custom UR Script execution

* Restarting the driver when robot closes the connection on script error.

The pipelines work in the way that if the connection is
is closed by the control PC, it will not be re-established. This
happens for example if you use the URScript topic and upload
script that does not compile. The robot will then close the
connection, the pipeline will close and any subsequent
uploads will fail and noone realises there is a problem.

While we could re-establish the connection, I think much better
solution is to shutdown the driver in such case. This is much more
resilient behaviour as it will clean up any inconsistent driver state.

We can utilise "respawn" feature of ROS launch and restart such
driver automatically (launch files are updated as part of that change).

On top of "production" stability, it allows for much nicer development
workflow - you can use URScript topic for development of new scripts
and have the driver restart every time you make mistake.
Without it, any mistake requires restarting the driver manually.
This commit is contained in:
Jarek Potiuk
2018-01-02 20:22:55 +01:00
committed by Simon Rasmussen
parent 24eef75d72
commit 6950b3c4bd
20 changed files with 176 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/ros_main.cpp
View File

@@ -13,6 +13,7 @@
#include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ros/urscript_handler.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/factory.h"
#include "ur_modern_driver/ur/messages.h"
@@ -30,6 +31,7 @@ static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame");
static const std::string TCP_LINK_ARG("~tcp_link");
static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
static const std::string SHUTDOWN_ON_DISCONNECT_ARG("~shutdown_on_disconnect");
static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
@@ -51,8 +53,34 @@ public:
double max_vel_change;
int32_t reverse_port;
bool use_ros_control;
bool shutdown_on_disconnect;
};
class IgnorePipelineStoppedNotifier : public INotifier
{
public:
void started(std::string name){
LOG_INFO("Starting pipeline %s", name.c_str());
}
void stopped(std::string name){
LOG_INFO("Stopping pipeline %s", name.c_str());
}
};
class ShutdownOnPipelineStoppedNotifier : public INotifier
{
public:
void started(std::string name){
LOG_INFO("Starting pipeline %s", name.c_str());
}
void stopped(std::string name){
LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
ros::shutdown();
exit(1);
}
};
bool parse_args(ProgArgs &args)
{
if (!ros::param::get(IP_ADDR_ARG, args.host))
@@ -69,6 +97,7 @@ bool parse_args(ProgArgs &args)
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
ros::param::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "tool0");
ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS);
ros::param::param(SHUTDOWN_ON_DISCONNECT_ARG, args.shutdown_on_disconnect, true);
return true;
}
@@ -107,6 +136,7 @@ int main(int argc, char **argv)
TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
INotifier *notifier(nullptr);
ROSController *controller(nullptr);
ActionServer *action_server(nullptr);
if (args.use_ros_control)
@@ -124,8 +154,21 @@ int main(int argc, char **argv)
services.push_back(action_server);
}
URScriptHandler urscript_handler(*rt_commander);
services.push_back(&urscript_handler);
if (args.shutdown_on_disconnect)
{
LOG_INFO("Notifier: Pipeline disconnect will shutdown the node");
notifier = new ShutdownOnPipelineStoppedNotifier();
}
else
{
LOG_INFO("Notifier: Pipeline disconnect will be ignored.");
notifier = new IgnorePipelineStoppedNotifier();
}
MultiConsumer<RTPacket> rt_cons(rt_vec);
Pipeline<RTPacket> rt_pl(rt_prod, rt_cons);
Pipeline<RTPacket> rt_pl(rt_prod, rt_cons, "RTPacket", *notifier);
// Message packets
auto state_parser = factory.getStateParser();
@@ -137,7 +180,7 @@ int main(int argc, char **argv)
vector<IConsumer<StatePacket> *> state_vec{ &state_pub, &service_stopper };
MultiConsumer<StatePacket> state_cons(state_vec);
Pipeline<StatePacket> state_pl(state_prod, state_cons);
Pipeline<StatePacket> state_pl(state_prod, state_cons, "StatePacket", *notifier);
LOG_INFO("Starting main loop");