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

Refactored intialisation of ROSController/ActionServer

Browse Source
This commit is contained in:
Jarek Potiuk
2018-02-24 13:33:04 +01:00
parent db61edfe5b
commit 71bca7abf7
6 changed files with 24 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/ur_modern_driver/ros/action_server.h
View File

@@ -39,7 +39,7 @@ private:
std::condition_variable tj_cv_;
std::thread tj_thread_;
TrajectoryFollowerInterface& follower_;
ActionTrajectoryFollowerInterface& follower_;
RobotState state_;
std::array<double, 6> q_actual_, qd_actual_;
@@ -61,7 +61,7 @@ private:
bool updateState(RTShared& data);
public:
ActionServer(TrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
void start();
virtual void onRobotStateChange(RobotState state);

4
include/ur_modern_driver/ros/trajectory_follower_interface.h → include/ur_modern_driver/ros/action_trajectory_follower_interface.h
View File

@@ -22,11 +22,11 @@ struct TrajectoryPoint
}
};
class TrajectoryFollowerInterface
class ActionTrajectoryFollowerInterface
{
public:
virtual bool start() = 0;
virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
virtual void stop() = 0;
virtual ~TrajectoryFollowerInterface() {};
virtual ~ActionTrajectoryFollowerInterface() {};
};

4
include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h
View File

@@ -11,9 +11,9 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h"
#include "ur_modern_driver/ros/trajectory_follower_interface.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
class LowBandwidthTrajectoryFollower: public TrajectoryFollowerInterface
class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface
{
private:
std::atomic<bool> running_;

4
include/ur_modern_driver/ros/trajectory_follower.h
View File

@@ -11,9 +11,9 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h"
#include "ur_modern_driver/ros/trajectory_follower_interface.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
class TrajectoryFollower : public TrajectoryFollowerInterface
class TrajectoryFollower : public ActionTrajectoryFollowerInterface
{
private:
std::atomic<bool> running_;

2
src/ros/action_server.cpp
View File

@@ -1,7 +1,7 @@
#include "ur_modern_driver/ros/action_server.h"
#include <cmath>
ActionServer::ActionServer(TrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity)
ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity)
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1), false)
, joint_names_(joint_names)

32
src/ros_main.cpp
View File

@@ -138,35 +138,33 @@ int main(int argc, char **argv)
auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
TrajectoryFollowerInterface *traj_follower(nullptr);
if (args.use_lowbandwidth_trajectory_follower && !args.use_ros_control)
{
LOG_INFO("Use low bandwidth trajectory follower");
traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3());
// We are leaking the follower here, but it's OK as this is once-a-lifetime event
}
else
{
LOG_INFO("Use standard trajectory follower");
traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
// We are leaking the follower here, but it's OK as this is once-a-lifetime event
}
INotifier *notifier(nullptr);
ROSController *controller(nullptr);
ActionServer *action_server(nullptr);
if (args.use_ros_control)
{
LOG_INFO("ROS control enabled");
// Note - we are sure that TrajectoryFollower is used here (see the args.use_ros_control above)
controller = new ROSController(*rt_commander, *((TrajectoryFollower *) traj_follower), args.joint_names, args.max_vel_change, args.tcp_link);
TrajectoryFollower *traj_follower = new TrajectoryFollower(
*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
rt_vec.push_back(controller);
services.push_back(controller);
}
else
{
LOG_INFO("ActionServer enabled");
ActionTrajectoryFollowerInterface *traj_follower(nullptr);
if (args.use_lowbandwidth_trajectory_follower)
{
LOG_INFO("Use low bandwidth trajectory follower");
traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander,
local_ip, args.reverse_port,factory.isVersion3());
}
else
{
LOG_INFO("Use standard trajectory follower");
traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
}
action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity);
rt_vec.push_back(action_server);
services.push_back(action_server);