Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +02:00
Code Releases Activity

Renamed Safe Trajectory Follower to Low Bandwidth one

Browse Source
This commit is contained in:
Jarek Potiuk
2018-01-14 21:04:41 +01:00
parent 5dcef72415
commit a54b97e939
7 changed files with 33 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/ros/safe_trajectory_follower.cpp → src/ros/lowbandwidth_trajectory_follower.cpp
View File

@@ -1,4 +1,4 @@
#include "ur_modern_driver/ros/safe_trajectory_follower.h"
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include <endian.h>
#include <cmath>
#include <ros/ros.h>
@@ -16,7 +16,7 @@ static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}");
static const std::string REVERSE_IP("{{REVERSE_IP}}");
static const std::string REVERSE_PORT("{{REVERSE_PORT}}");
static const std::string POSITION_PROGRAM = R"(
def driveRobotSafeTrajectory():
def driveRobotLowBandwidthTrajectory():
global JOINT_NUM = 6
global TIME_INTERVAL = {{TIME_INTERVAL}}
@@ -359,7 +359,7 @@ end
)";
SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
bool version_3)
: running_(false)
, commander_(commander)
@@ -385,7 +385,7 @@ SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::stri
std::string res(POSITION_PROGRAM);
std::ostringstream out;
if (!version_3) {
LOG_ERROR("Safe Trajectory Follower only works for interface version > 3");
LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
std::exit(-1);
}
res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_));
@@ -405,12 +405,12 @@ SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::stri
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
LOG_INFO("Safe Trajectory Follower is initialized!");
LOG_INFO("Low Bandwidth Trajectory Follower is initialized!");
}
bool SafeTrajectoryFollower::start()
bool LowBandwidthTrajectoryFollower::start()
{
LOG_INFO("Starting SafeTrajectoryFollower");
LOG_INFO("Starting LowBandwidthTrajectoryFollower");
if (running_)
return true; // not sure
@@ -435,7 +435,7 @@ bool SafeTrajectoryFollower::start()
return (running_ = true);
}
bool SafeTrajectoryFollower::execute(const std::array<double, 6> &positions,
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
const std::array<double, 6> &velocities,
double sample_number, double time_in_seconds)
{
@@ -468,7 +468,7 @@ bool SafeTrajectoryFollower::execute(const std::array<double, 6> &positions,
return server_.write(buf, strlen(formatted_message) + 1, written);
}
bool SafeTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
{
if (!running_)
return false;
@@ -505,7 +505,7 @@ bool SafeTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, s
return res;
}
void SafeTrajectoryFollower::stop()
void LowBandwidthTrajectoryFollower::stop()
{
if (!running_)
return;

14
src/ros_main.cpp
View File

@@ -13,7 +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/safe_trajectory_follower.h"
#include "ur_modern_driver/ros/lowbandwidth_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"
@@ -26,7 +26,7 @@
static const std::string IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port");
static const std::string ROS_CONTROL_ARG("~use_ros_control");
static const std::string SAFE_TRAJECTORY_FOLLOWER("~use_safe_trajectory_follower");
static const std::string LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_trajectory_follower");
static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change");
static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame");
@@ -55,7 +55,7 @@ public:
double max_vel_change;
int32_t reverse_port;
bool use_ros_control;
bool use_safe_trajectory_follower;
bool use_lowbandwidth_trajectory_follower;
bool shutdown_on_disconnect;
};
@@ -95,7 +95,7 @@ bool parse_args(ProgArgs &args)
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0);
ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
ros::param::param(SAFE_TRAJECTORY_FOLLOWER, args.use_safe_trajectory_follower, false);
ros::param::param(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_trajectory_follower, false);
ros::param::param(PREFIX_ARG, args.prefix, std::string());
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
@@ -140,10 +140,10 @@ int main(int argc, char **argv)
TrajectoryFollowerInterface *traj_follower(nullptr);
if (args.use_safe_trajectory_follower && !args.use_ros_control)
if (args.use_lowbandwidth_trajectory_follower && !args.use_ros_control)
{
LOG_INFO("Use safe trajectory follower");
traj_follower = new SafeTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3());
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