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

Merge pull request #9 from NoMagicAi/SAFE_TRAJECTORY_FOLLOWER

Browse Source 
Adds Low Bandwith Trajectory Follower implementation
This commit is contained in:
G.A. vd. Hoorn
2018-09-27 16:06:39 +02:00
committed by GitHub
parent a2f5f4bed5 c72ec5bded
commit 50bd24dd3b
23 changed files with 791 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -4,3 +4,4 @@ Makefile
cmake_install.cmake cmake_install.cmake
install_manifest.txt install_manifest.txt
*~ *~
.idea

1
CMakeLists.txt
View File

@@ -172,6 +172,7 @@ set(${PROJECT_NAME}_SOURCES
src/ros/rt_publisher.cpp src/ros/rt_publisher.cpp
src/ros/service_stopper.cpp src/ros/service_stopper.cpp
src/ros/trajectory_follower.cpp src/ros/trajectory_follower.cpp
src/ros/lowbandwidth_trajectory_follower.cpp
src/ros/urscript_handler.cpp src/ros/urscript_handler.cpp
src/ur/stream.cpp src/ur/stream.cpp
src/ur/server.cpp src/ur/server.cpp

37
README.md
View File

@@ -22,9 +22,9 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
* Besides this, the driver subscribes to two new topics: * Besides this, the driver subscribes to two new topics:
* */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like. * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Intended for sending movel/movej commands directly to the robot, conveyor tracking and the like.
* */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTrajectoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
* Added support for ros_control. * Added support for ros_control.
* As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time. * As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time.
@@ -39,6 +39,39 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
* Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults. * Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults.
* **Low Bandwidth Trajectory Follower** mode of execution. (only works when `ros_control` is set to `false`)In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features:
* It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control*
* It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript.
* It is much more resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup.
* Other methods of controlling the robot by ur_modern_driver are very fragile and require low-latency kernel, separated wired network and ideally no other network activity on the PC where ur_modern_driver runs.
* **Low Bandwidth Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Low Bandwith Trajectory Follower predictability of the move has priority over move execution time.
* The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Low Bandwidth Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC.
* Due to communication optimisations and **Low Bandwidth Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods.
* Time flow for the URScript program might be independent from "real time" and can be controlled to speed up or slow down execution of planned moves.
* The Low Bandwidth Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters)
* There are several parameters that you can use to control Low Bandwidth Trajectory Follower's behaviour:
* **use_lowbandwidth_trajectory_follower** - should be set to `true` to enable the follower
* **time_interval** - time interval (in seconds) this is 'simulated' time for interpolation steps of the move. Together with *servoj_time* it can be used to change speed of moves even after they are planned by MoveIt. Default value is 0.008
* **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control)
* **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency)
* **max_waiting_time** - maximum time in seconds (real time) to wait for instructions from the drive before move is aborted. Defaults to 2 seconds.
* **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes)
* **max_joint_difference** - maximum allowed difference between target and actual joints - checked at every trajectory step
Here are some examples of manipulating the time flow for **Low Bandwidth Trajectory Follower** mode. You can use other settings but you should do it on your own risk.
* Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned
* Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way)
* Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is.
NOTE! In case you use Low Bandwidth Trajectory Follower and you experience MoveIt to cancel robot moves prematurely
because of too long move duration, you should increase tolerance of duration monitoring of MoveIt trajectory execution
You can find the configuration usually in trajectory_execution.launch.xml in generated moveit config - there are
parameters that configure scaling and margin for allowed execution time among others.
The relevant parameters are `trajectory_execution/allowed_execution_duration_scaling` (default 1.2) and
`trajectory_execution/allowed_goal_duration_margin` (default 0.5). The first one is factor that scales execution time,
the second is margin that is added on top of the scaled one. You can increase either of those values to make moveit
executor more "tolerant" to execution delays. There is also another parameter:
`trajectory_execution/execution_duration_monitoring`. You can set it to false to disable duration monitoring completely.
## Installation ## Installation
**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** **As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.**

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

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

32
include/ur_modern_driver/ros/action_trajectory_follower_interface.h Normal file
View File

@@ -0,0 +1,32 @@
#pragma once
#include <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <vector>
struct TrajectoryPoint
{
std::array<double, 6> positions;
std::array<double, 6> velocities;
std::chrono::microseconds time_from_start;
TrajectoryPoint()
{
}
TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
: positions(pos), velocities(vel), time_from_start(tfs)
{
}
};
class ActionTrajectoryFollowerInterface
{
public:
virtual bool start() = 0;
virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
virtual void stop() = 0;
virtual ~ActionTrajectoryFollowerInterface() {};
};

41
include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h Normal file
View File

@@ -0,0 +1,41 @@
#pragma once
#include <inttypes.h>
#include <array>
#include <atomic>
#include <cstddef>
#include <cstring>
#include <string>
#include <thread>
#include <vector>
#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/action_trajectory_follower_interface.h"
class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface
{
private:
std::atomic<bool> running_;
std::array<double, 6> last_positions_;
URCommander &commander_;
URServer server_;
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \
servoj_gain_, servoj_lookahead_time_, max_joint_difference_;
std::string program_;
bool execute(const std::array<double, 6> &positions,
const std::array<double, 6> &velocities,
double sample_number, double time_in_seconds);
public:
LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
bool start();
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
void stop();
virtual ~LowBandwidthTrajectoryFollower() {};
};

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

@@ -11,24 +11,9 @@
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h" #include "ur_modern_driver/ur/server.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
struct TrajectoryPoint class TrajectoryFollower : public ActionTrajectoryFollowerInterface
{
std::array<double, 6> positions;
std::array<double, 6> velocities;
std::chrono::microseconds time_from_start;
TrajectoryPoint()
{
}
TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
: positions(pos), velocities(vel), time_from_start(tfs)
{
}
};
class TrajectoryFollower
{ {
private: private:
std::atomic<bool> running_; std::atomic<bool> running_;
@@ -57,5 +42,6 @@ public:
bool execute(std::array<double, 6> &positions); bool execute(std::array<double, 6> &positions);
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt); bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
void stop(); void stop();
void interrupt();
virtual ~TrajectoryFollower() {};
}; };

1
include/ur_modern_driver/tcp_socket.h
View File

@@ -46,6 +46,7 @@ public:
std::string getIP(); std::string getIP();
bool read(char *character);
bool read(uint8_t *buf, size_t buf_len, size_t &read); bool read(uint8_t *buf, size_t buf_len, size_t &read);
bool write(const uint8_t *buf, size_t buf_len, size_t &written); bool write(const uint8_t *buf, size_t buf_len, size_t &written);

3
include/ur_modern_driver/ur/server.h
View File

@@ -8,6 +8,8 @@
#include <string> #include <string>
#include "ur_modern_driver/tcp_socket.h" #include "ur_modern_driver/tcp_socket.h"
#define MAX_SERVER_BUF_LEN 50
class URServer : private TCPSocket class URServer : private TCPSocket
{ {
private: private:
@@ -24,5 +26,6 @@ public:
bool bind(); bool bind();
bool accept(); bool accept();
void disconnectClient(); void disconnectClient();
bool readLine(char* buffer, size_t buf_len);
bool write(const uint8_t *buf, size_t buf_len, size_t &written); bool write(const uint8_t *buf, size_t buf_len, size_t &written);
}; };

21
launch/ur10_bringup.launch
View File

@@ -14,6 +14,16 @@
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/> <arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" /> <arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<!-- robot model --> <!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch"> <include file="$(find ur_description)/launch/ur10_upload.launch">
@@ -25,6 +35,17 @@
<arg name="robot_ip" value="$(arg robot_ip)"/> <arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>

23
launch/ur10_bringup_compatible.launch
View File

@@ -13,6 +13,17 @@
<arg name="limited" default="false"/> <arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/> <arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.08"/>
<arg name="servoj_time" default="0.08" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<!-- robot model --> <!-- robot model -->
@@ -25,7 +36,17 @@
<arg name="robot_ip" value="$(arg robot_ip)"/> <arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="servoj_time" value="0.08" /> <arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>

20
launch/ur10_bringup_joint_limited.launch
View File

@@ -13,6 +13,16 @@
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/> <arg name="max_payload" default="10.0"/>
<arg name="prefix" default="" /> <arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<include file="$(find ur_modern_driver)/launch/ur10_bringup.launch"> <include file="$(find ur_modern_driver)/launch/ur10_bringup.launch">
@@ -21,6 +31,16 @@
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" /> <arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>
</launch> </launch>

20
launch/ur3_bringup.launch
View File

@@ -14,6 +14,16 @@
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/> <arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" /> <arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<!-- robot model --> <!-- robot model -->
<include file="$(find ur_description)/launch/ur3_upload.launch"> <include file="$(find ur_description)/launch/ur3_upload.launch">
@@ -26,6 +36,16 @@
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" /> <arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>

20
launch/ur3_bringup_joint_limited.launch
View File

@@ -13,6 +13,16 @@
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="3.0"/> <arg name="max_payload" default="3.0"/>
<arg name="prefix" default="" /> <arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<include file="$(find ur_modern_driver)/launch/ur3_bringup.launch"> <include file="$(find ur_modern_driver)/launch/ur3_bringup.launch">
@@ -21,6 +31,16 @@
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" /> <arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>
</launch> </launch>

22
launch/ur5_bringup.launch
View File

@@ -14,6 +14,16 @@
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/> <arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" /> <arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<!-- robot model --> <!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch"> <include file="$(find ur_description)/launch/ur5_upload.launch">
@@ -22,10 +32,20 @@
<!-- ur common --> <!-- ur common -->
<include file="$(find ur_modern_driver)/launch/ur_common.launch"> <include file="$(find ur_modern_driver)/launch/ur_common.launch">
<arg name="prefix" value="$(arg prefix)" />
<arg name="robot_ip" value="$(arg robot_ip)"/> <arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>

23
launch/ur5_bringup_compatible.launch
View File

@@ -13,6 +13,17 @@
<arg name="limited" default="false"/> <arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/> <arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.08"/>
<arg name="servoj_time" default="0.08" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<!-- robot model --> <!-- robot model -->
@@ -25,7 +36,17 @@
<arg name="robot_ip" value="$(arg robot_ip)"/> <arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="servoj_time" value="0.08" /> <arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>

20
launch/ur5_bringup_joint_limited.launch
View File

@@ -13,6 +13,16 @@
<arg name="min_payload" default="0.0"/> <arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/> <arg name="max_payload" default="5.0"/>
<arg name="prefix" default="" /> <arg name="prefix" default="" />
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
<include file="$(find ur_modern_driver)/launch/ur5_bringup.launch"> <include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
@@ -21,6 +31,16 @@
<arg name="min_payload" value="$(arg min_payload)"/> <arg name="min_payload" value="$(arg min_payload)"/>
<arg name="max_payload" value="$(arg max_payload)"/> <arg name="max_payload" value="$(arg max_payload)"/>
<arg name="prefix" value="$(arg prefix)" /> <arg name="prefix" value="$(arg prefix)" />
<arg name="use_lowbandwidth_trajectory_follower" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<arg name="time_interval" value="$(arg time_interval)"/>
<arg name="servoj_time" value="$(arg servoj_time)" />
<arg name="servoj_time_waiting" default="$(arg servoj_time_waiting)" />
<arg name="max_waiting_time" value="$(arg max_waiting_time)" />
<arg name="servoj_gain" value="$(arg servoj_gain)" />
<arg name="servoj_lookahead_time" value="$(arg servoj_lookahead_time)" />
<arg name="max_joint_difference" value="$(arg max_joint_difference)" />
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="tool_frame" value="$(arg tool_frame)" />
<arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/> <arg name="shutdown_on_disconnect" value="$(arg shutdown_on_disconnect)"/>
</include> </include>
</launch> </launch>

16
launch/ur_common.launch
View File

@@ -12,7 +12,15 @@
<arg name="min_payload" /> <arg name="min_payload" />
<arg name="max_payload" /> <arg name="max_payload" />
<arg name="prefix" default="" /> <arg name="prefix" default="" />
<arg name="use_ros_control" default="false"/>
<arg name="use_lowbandwidth_trajectory_follower" default="false"/>
<arg name="time_interval" default="0.008"/>
<arg name="servoj_time" default="0.008" /> <arg name="servoj_time" default="0.008" />
<arg name="servoj_time_waiting" default="0.001" />
<arg name="max_waiting_time" default="2.0" />
<arg name="servoj_gain" default="100." />
<arg name="servoj_lookahead_time" default="1." />
<arg name="max_joint_difference" default="0.01" />
<arg name="base_frame" default="$(arg prefix)base" /> <arg name="base_frame" default="$(arg prefix)base" />
<arg name="tool_frame" default="$(arg prefix)tool0_controller" /> <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
<arg name="shutdown_on_disconnect" default="true" /> <arg name="shutdown_on_disconnect" default="true" />
@@ -30,10 +38,18 @@
<!-- copy the specified IP address to be consistant with ROS-Industrial spec. --> <!-- copy the specified IP address to be consistant with ROS-Industrial spec. -->
<param name="prefix" type="str" value="$(arg prefix)" /> <param name="prefix" type="str" value="$(arg prefix)" />
<param name="robot_ip_address" type="str" value="$(arg robot_ip)" /> <param name="robot_ip_address" type="str" value="$(arg robot_ip)" />
<param name="use_ros_control" type="bool" value="$(arg use_ros_control)"/>
<param name="use_lowbandwidth_trajectory_follower" type="bool" value="$(arg use_lowbandwidth_trajectory_follower)"/>
<param name="min_payload" type="double" value="$(arg min_payload)" /> <param name="min_payload" type="double" value="$(arg min_payload)" />
<param name="max_payload" type="double" value="$(arg max_payload)" /> <param name="max_payload" type="double" value="$(arg max_payload)" />
<param name="max_velocity" type="double" value="$(arg max_velocity)" /> <param name="max_velocity" type="double" value="$(arg max_velocity)" />
<param name="time_interval" type="double" value="$(arg time_interval)" />
<param name="servoj_time" type="double" value="$(arg servoj_time)" /> <param name="servoj_time" type="double" value="$(arg servoj_time)" />
<param name="servoj_time_waiting" type="double" value="$(arg servoj_time_waiting)" />
<param name="max_waiting_time" type="double" value="$(arg max_waiting_time)" />
<param name="servoj_gain" type="double" value="$(arg servoj_gain)" />
<param name="servoj_lookahead_time" type="double" value="$(arg servoj_lookahead_time)" />
<param name="max_joint_difference" type="double" value="$(arg max_joint_difference)" />
<param name="base_frame" type="str" value="$(arg base_frame)"/> <param name="base_frame" type="str" value="$(arg base_frame)"/>
<param name="tool_frame" type="str" value="$(arg tool_frame)"/> <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
<param name="require_activation" type="str" value="$(arg require_activation)" /> <param name="require_activation" type="str" value="$(arg require_activation)" />

3
src/ros/action_server.cpp
View File

@@ -1,7 +1,7 @@
#include "ur_modern_driver/ros/action_server.h" #include "ur_modern_driver/ros/action_server.h"
#include <cmath> #include <cmath>
ActionServer::ActionServer(TrajectoryFollower& 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), : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1), false) boost::bind(&ActionServer::onCancel, this, _1), false)
, joint_names_(joint_names) , joint_names_(joint_names)
@@ -307,6 +307,7 @@ void ActionServer::trajectoryThread()
Result res; Result res;
LOG_INFO("Attempting to start follower %p", &follower_);
if (follower_.start()) if (follower_.start())
{ {
if (follower_.execute(trajectory, interrupt_traj_)) if (follower_.execute(trajectory, interrupt_traj_))

386
src/ros/lowbandwidth_trajectory_follower.cpp Normal file
View File

@@ -0,0 +1,386 @@
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include <endian.h>
#include <cmath>
#include <ros/ros.h>
static const std::array<double, 6> EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}");
static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}");
static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}");
static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}");
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 MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}");
static const std::string POSITION_PROGRAM = R"(
def driveRobotLowBandwidthTrajectory():
global JOINT_NUM = 6
global TIME_INTERVAL = {{TIME_INTERVAL}}
global SERVOJ_TIME = {{SERVOJ_TIME}}
global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}}
global MAX_WAITING_TIME = {{MAX_WAITING_TIME}}
global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global SERVOJ_GAIN = {{SERVOJ_GAIN}}
global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}}
global CONNECTION_NAME = "reverse_connection"
global REVERSE_IP = "{{REVERSE_IP}}"
global REVERSE_PORT = {{REVERSE_PORT}}
global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}}
global g_position_previous = EMPTY_VALUES
global g_position_target = EMPTY_VALUES
global g_position_next = EMPTY_VALUES
global g_velocity_previous = EMPTY_VALUES
global g_velocity_target = EMPTY_VALUES
global g_velocity_next = EMPTY_VALUES
global g_time_previous = 0.0
global g_time_target = 0.0
global g_time_next = 0.0
global g_num_previous = -1
global g_num_target = -1
global g_num_next = -1
global g_received_waypoints_number = -1
global g_requested_waypoints_number = -1
global g_total_elapsed_time = 0
global g_stopping = False
def send_message(message):
socket_send_string(message, CONNECTION_NAME)
socket_send_byte(10, CONNECTION_NAME)
end
def is_waypoint_sentinel(waypoint):
local l_previous_index = 2
while l_previous_index < 1 + JOINT_NUM * 2 + 2:
if waypoint[l_previous_index] != 0.0:
return False
end
l_previous_index = l_previous_index + 1
end
return True
end
def is_final_position_reached(position):
local l_current_position = get_actual_joint_positions()
local l_index = 0
while l_index < JOINT_NUM:
if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE:
return False
end
l_index = l_index + 1
end
return True
end
def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel):
local a = start_pos
local b = l_start_vel
local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2)
local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3)
return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3)
end
def add_next_waypoint(waypoint):
enter_critical
g_position_previous = g_position_target
g_velocity_previous = g_velocity_target
g_time_previous = g_time_target
g_num_previous = g_num_target
g_position_target = g_position_next
g_velocity_target = g_velocity_next
g_time_target = g_time_next
g_num_target = g_num_next
g_num_next = waypoint[1]
g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]]
g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]]
g_time_next = waypoint[14]
g_received_waypoints_number = g_num_next
exit_critical
end
thread controllingThread():
local l_received_waypoints_number = -1
local l_requested_waypoints_number = -1
local l_stopped = False
local l_current_position = get_actual_joint_positions()
enter_critical
g_requested_waypoints_number = 2
exit_critical
while True:
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
exit_critical
local l_max_waiting_time_left = MAX_WAITING_TIME
while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0:
servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
l_received_waypoints_number = g_received_waypoints_number
exit_critical
l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING
end
if l_max_waiting_time_left <= 0:
textmsg("Closing the connection on waiting too long.")
socket_close(CONNECTION_NAME)
halt
end
enter_critical
local l_start_pos = g_position_previous
local l_start_vel = g_velocity_previous
local l_start_time = g_time_previous
local l_start_num= g_num_previous
local l_end_pos = g_position_target
local l_end_vel = g_velocity_target
local l_end_time = g_time_target
local l_end_num = g_num_target
local l_total_elapsed_time = g_total_elapsed_time
local l_stopping_after_next_interpolation = g_stopping
g_requested_waypoints_number = g_requested_waypoints_number + 1
exit_critical
l_current_position = l_start_pos
local l_total_segment_time = l_end_time - l_start_time
while l_total_elapsed_time <= l_end_time:
local l_segment_elapsed_time = l_total_elapsed_time - l_start_time
j = 0
while j < JOINT_NUM:
l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j])
j = j + 1
end
servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL
l_total_elapsed_time = g_total_elapsed_time
exit_critical
end
if l_stopping_after_next_interpolation:
while not is_final_position_reached(l_end_pos):
textmsg("Catching up on final position not reached first time.")
servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
end
textmsg("Finishing the controlling thread. Final position reached.")
break
end
end
end
thread sendingThread():
local controlling_thread = run controllingThread()
local l_sent_waypoints_number = -1
local l_requested_waypoints_number = -1
local l_stopping = False
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
l_stopping = g_stopping
exit_critical
while not l_stopping:
while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping:
sleep(SERVOJ_TIME_WAITING)
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
l_stopping = g_stopping
exit_critical
end
if l_stopping:
break
end
send_message(l_sent_waypoints_number + 1)
l_sent_waypoints_number = l_sent_waypoints_number + 1
end
join controlling_thread
end
thread receivingThread():
local sending_thread = run sendingThread()
while True:
waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME)
if waypoint_received[0] == 0:
textmsg("Not received trajectory for the last 2 seconds. Quitting")
enter_critical
g_stopping = True
exit_critical
break
elif waypoint_received[0] != JOINT_NUM * 2 + 2:
textmsg("Received wrong number of floats in trajectory. This is certainly not OK.")
textmsg(waypoint_received[0])
enter_critical
g_stopping = True
exit_critical
break
elif is_waypoint_sentinel(waypoint_received):
add_next_waypoint(waypoint_received)
enter_critical
g_stopping = True
g_received_waypoints_number = g_received_waypoints_number + 1
exit_critical
break
end
add_next_waypoint(waypoint_received)
end
join sending_thread
end
socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME)
receiving_thread = run receivingThread()
join receiving_thread
socket_close(CONNECTION_NAME)
textmsg("Exiting the program")
end
)";
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
, time_interval_(0.008)
, servoj_time_(0.008)
, servoj_time_waiting_(0.001)
, max_waiting_time_(2.0)
, servoj_gain_(300.0)
, servoj_lookahead_time_(0.03)
, max_joint_difference_(0.01)
{
ros::param::get("~time_interval", time_interval_);
ros::param::get("~servoj_time", servoj_time_);
ros::param::get("~servoj_time_waiting", servoj_time_waiting_);
ros::param::get("~max_waiting_time", max_waiting_time_);
ros::param::get("~servoj_gain", servoj_gain_);
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
ros::param::get("~max_joint_difference", max_joint_difference_);
std::string res(POSITION_PROGRAM);
std::ostringstream out;
if (!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_));
res.replace(res.find(SERVOJ_TIME_WAITING), SERVOJ_TIME_WAITING.length(), std::to_string(servoj_time_waiting_));
res.replace(res.find(SERVOJ_TIME), SERVOJ_TIME.length(), std::to_string(servoj_time_));
res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_));
res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_));
res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_));
res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip);
res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port));
res.replace(res.find(MAX_JOINT_DIFFERENCE), MAX_JOINT_DIFFERENCE.length(), std::to_string(max_joint_difference_));
program_ = res;
if (!server_.bind())
{
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
LOG_INFO("Low Bandwidth Trajectory Follower is initialized!");
}
bool LowBandwidthTrajectoryFollower::start()
{
LOG_INFO("Starting LowBandwidthTrajectoryFollower");
if (running_)
return true; // not sure
LOG_INFO("Uploading trajectory program to robot");
if (!commander_.uploadProg(program_))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_DEBUG("Awaiting incoming robot connection");
if (!server_.accept())
{
LOG_ERROR("Failed to accept incoming robot connection");
return false;
}
LOG_DEBUG("Robot successfully connected");
return (running_ = true);
}
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
const std::array<double, 6> &velocities,
double sample_number, double time_in_seconds)
{
if (!running_)
return false;
std::ostringstream out;
out << "(";
out << sample_number << ",";
for (auto const &pos: positions)
{
out << pos << ",";
}
for (auto const &vel: velocities)
{
out << vel << ",";
}
out << time_in_seconds << ")\r\n";
// I know it's ugly but it's the most efficient and fastest way
// We have only ASCII characters and we can cast char -> uint_8
const std::string tmp = out.str();
const char *formatted_message = tmp.c_str();
const uint8_t *buf = (uint8_t *) formatted_message;
size_t written;
LOG_DEBUG("Sending message %s", formatted_message);
return server_.write(buf, strlen(formatted_message) + 1, written);
}
bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
{
if (!running_)
return false;
bool finished = false;
char* line[MAX_SERVER_BUF_LEN];
bool res = true;
while (!finished && !interrupt)
{
if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN))
{
LOG_DEBUG("Connection closed. Finishing!");
finished = true;
break;
}
unsigned int message_num=atoi((const char *) line);
LOG_DEBUG("Received request %i", message_num);
if (message_num < trajectory.size())
{
res = execute(trajectory[message_num].positions, trajectory[message_num].velocities,
message_num, trajectory[message_num].time_from_start.count() / 1e6);
} else
{
res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
}
if (!res)
{
finished = true;
}
}
return res;
}
void LowBandwidthTrajectoryFollower::stop()
{
if (!running_)
return;
server_.disconnectClient();
running_ = false;
}

24
src/ros_main.cpp
View File

@@ -13,6 +13,7 @@
#include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include "ur_modern_driver/ros/urscript_handler.h" #include "ur_modern_driver/ros/urscript_handler.h"
#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/factory.h"
@@ -25,6 +26,7 @@
static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string IP_ADDR_ARG("~robot_ip_address");
static const std::string REVERSE_PORT_ARG("~reverse_port"); static const std::string REVERSE_PORT_ARG("~reverse_port");
static const std::string ROS_CONTROL_ARG("~use_ros_control"); static const std::string ROS_CONTROL_ARG("~use_ros_control");
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 MAX_VEL_CHANGE_ARG("~max_vel_change");
static const std::string PREFIX_ARG("~prefix"); static const std::string PREFIX_ARG("~prefix");
static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string BASE_FRAME_ARG("~base_frame");
@@ -53,6 +55,7 @@ public:
double max_vel_change; double max_vel_change;
int32_t reverse_port; int32_t reverse_port;
bool use_ros_control; bool use_ros_control;
bool use_lowbandwidth_trajectory_follower;
bool shutdown_on_disconnect; bool shutdown_on_disconnect;
}; };
@@ -92,6 +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_vel_change, 15.0); // rad/s
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0); 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(ROS_CONTROL_ARG, args.use_ros_control, 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(PREFIX_ARG, args.prefix, std::string());
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); 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"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
@@ -134,22 +138,34 @@ int main(int argc, char **argv)
auto rt_commander = factory.getCommander(rt_stream); auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub }; vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
INotifier *notifier(nullptr); INotifier *notifier(nullptr);
ROSController *controller(nullptr); ROSController *controller(nullptr);
ActionServer *action_server(nullptr); ActionServer *action_server(nullptr);
if (args.use_ros_control) if (args.use_ros_control)
{ {
LOG_INFO("ROS control enabled"); LOG_INFO("ROS control enabled");
controller = new ROSController(*rt_commander, 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); rt_vec.push_back(controller);
services.push_back(controller); services.push_back(controller);
} }
else else
{ {
LOG_INFO("ActionServer enabled"); LOG_INFO("ActionServer enabled");
action_server = new ActionServer(traj_follower, args.joint_names, args.max_velocity); 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); rt_vec.push_back(action_server);
services.push_back(action_server); services.push_back(action_server);
} }

12
src/tcp_socket.cpp
View File

@@ -90,7 +90,7 @@ void TCPSocket::close()
if (state_ != SocketState::Connected) if (state_ != SocketState::Connected)
return; return;
state_ = SocketState::Closed; state_ = SocketState::Closed;
::shutdown(socket_fd_, SHUT_RDWR); ::close(socket_fd_);
socket_fd_ = -1; socket_fd_ = -1;
} }
@@ -111,6 +111,16 @@ std::string TCPSocket::getIP()
return std::string(buf); return std::string(buf);
} }
bool TCPSocket::read(char *character)
{
size_t read_chars;
// It's inefficient, but in our case we read very small messages
// and the overhead connected with reading character by character is
// negligible - adding buffering would complicate the code needlessly.
return read((uint8_t *) character, 1, read_chars);
}
bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
{ {
read = 0; read = 0;

41
src/ur/server.cpp
View File

@@ -86,3 +86,44 @@ bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
{ {
return client_.write(buf, buf_len, written); return client_.write(buf, buf_len, written);
} }
bool URServer::readLine(char* buffer, size_t buf_len)
{
char *current_pointer = buffer;
char ch;
size_t total_read;
if (buf_len <= 0 || buffer == NULL) {
return false;
}
total_read = 0;
for (;;) {
if (client_.read(&ch))
{
if (total_read < buf_len - 1) // just in case ...
{
total_read ++;
*current_pointer++ = ch;
}
if (ch == '\n')
{
break;
}
}
else
{
if (total_read == 0)
{
return false;
}
else
{
break;
}
}
}
*current_pointer = '\0';
return true;
}