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

Cleaned up a bit and removed a few global vars

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-08 12:28:33 +02:00
parent 3e7d9042d1
commit 401a16fbd4
9 changed files with 1093 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

209
CMakeLists.txt Normal file
View File

@@ -0,0 +1,209 @@
cmake_minimum_required(VERSION 2.8.3)
project(ur_modern_driver)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
actionlib
control_msgs
geometry_msgs
roscpp
sensor_msgs
trajectory_msgs
ur_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# control_msgs# geometry_msgs# sensor_msgs# trajectory_msgs# ur_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES ur_modern_driver
# CATKIN_DEPENDS actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
# check c++11 / c++0x
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "-std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "-std=c++0x")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(ur_modern_driver
# src/${PROJECT_NAME}/ur_modern_driver.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(ur_modern_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
set(${PROJECT_NAME}_SOURCES
src/ur_ros_wrapper.cpp
src/ur_realtime_communication.cpp
src/ur_driver.cpp
src/robot_state_RT.cpp)
add_executable(ur_driver ${${PROJECT_NAME}_SOURCES})
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(ur_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(ur_driver
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ur_modern_driver ur_driver
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_modern_driver.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

101
include/ur_modern_driver/robot_state_RT.h Normal file
View File

@@ -0,0 +1,101 @@
/*
* robotStateRT.h
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
* ----------------------------------------------------------------------------
*/
#ifndef ROBOTSTATERT_H_
#define ROBOTSTATERT_H_
#include <inttypes.h>
#include <vector>
#include <stdlib.h>
#include <string.h>
#include <mutex>
#include <condition_variable>
class RobotStateRT {
private:
double version_; //protocol version
double time_; //Time elapsed since the controller was started
std::vector<double> q_target_; //Target joint positions
std::vector<double> qd_target_; //Target joint velocities
std::vector<double> qdd_target_; //Target joint accelerations
std::vector<double> i_target_; //Target joint currents
std::vector<double> m_target_; //Target joint moments (torques)
std::vector<double> q_actual_; //Actual joint positions
std::vector<double> qd_actual_; //Actual joint velocities
std::vector<double> i_actual_; //Actual joint currents
std::vector<double> i_control_; //Joint control currents
std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
std::vector<double> tcp_force_; //Generalised forces in the TC
std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates
std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
double controller_timer_; //Controller realtime thread execution time
double robot_mode_; //Robot mode
std::vector<double> joint_modes_; //Joint control modes
double safety_mode_; //Safety mode
std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7)
double speed_scaling_; //Speed scaling of the trajectory limiter
double linear_momentum_norm_; //Norm of Cartesian linear momentum
double v_main_; //Masterboard: Main voltage
double v_robot_; //Matorborad: Robot voltage (48V)
double i_robot_; //Masterboard: Robot current
std::vector<double> v_actual_; //Actual joint voltages
std::mutex val_lock_; // Locks the variables while unpack parses data;
std::condition_variable* pMsg_cond_; //Signals that new vars are available
bool new_data_available_; //to avoid spurious wakes
std::vector<double> unpackVector(uint8_t * buf, int start_index,
int nr_of_vals);
std::vector<bool> unpackDigitalInputBits(int64_t data);
double ntohd(uint64_t nf);
public:
RobotStateRT(std::condition_variable& msg_cond);
double getVersion();
double getTime();
std::vector<double> getQTarget();
std::vector<double> getQdTarget();
std::vector<double> getQddTarget();
std::vector<double> getITarget();
std::vector<double> getMTarget();
std::vector<double> getQActual();
std::vector<double> getQdActual();
std::vector<double> getIActual();
std::vector<double> getIControl();
std::vector<double> getToolVectorActual();
std::vector<double> getTcpSpeedActual();
std::vector<double> getTcpForce();
std::vector<double> getToolVectorTarget();
std::vector<double> getTcpSpeedTarget();
std::vector<bool> getDigitalInputBits();
std::vector<double> getMotorTemperatures();
double getControllerTimer();
double getRobotMode();
std::vector<double> getJointModes();
double getSafety_mode();
std::vector<double> getToolAccelerometerValues();
double getSpeedScaling();
double getLinearMomentumNorm();
double getVMain();
double getVRobot();
double getIRobot();
bool getNewDataAvailable();
void finishedReading();
std::vector<double> getVActual();
void unpack(uint8_t * buf);
};
#endif /* ROBOTSTATERT_H_ */

32
include/ur_modern_driver/ur_driver.h Normal file
View File

@@ -0,0 +1,32 @@
/*
* ur_driver
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
* ----------------------------------------------------------------------------
*/
#ifndef UR_DRIVER_H_
#define UR_DRIVER_H_
#include <mutex>
#include <condition_variable>
#include "ur_realtime_communication.h"
class UrDriver {
public:
UrRealtimeCommunication* rt_interface_;
UrDriver(std::condition_variable& msg_cond, std::string host,
uint safety_count_max = 12);
void start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
};
#endif /* UR_DRIVER_H_ */

62
include/ur_modern_driver/ur_realtime_communication.h Normal file
View File

@@ -0,0 +1,62 @@
/*
* ur_realtime_communication.h
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
* ----------------------------------------------------------------------------
*/
#ifndef UR_REALTIME_COMMUNICATION_H_
#define UR_REALTIME_COMMUNICATION_H_
#include "robot_state_RT.h"
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <iostream>
#include <unistd.h>
class UrRealtimeCommunication {
private:
const double SAMPLETIME_;
uint safety_count_max_;
int sockfd_;
struct sockaddr_in serv_addr_;
struct hostent *server_;
bool keepalive_;
std::thread comThread_;
int flag_;
std::recursive_mutex command_string_lock_;
std::string command_;
uint safety_count_;
void run();
public:
bool connected_;
RobotStateRT* robot_state_;
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
uint safety_count_max = 12);
void start();
void halt();
void setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc = 100.);
void addCommandToQueue(std::string inp);
void setSafetyCountMax(uint inp);
};
#endif /* UR_REALTIME_COMMUNICATION_H_ */

64
package.xml Normal file
View File

@@ -0,0 +1,64 @@
<?xml version="1.0"?>
<package>
<name>ur_modern_driver</name>
<version>0.0.1</version>
<description>The new driver for the UR3/UR5/UR10 robot arms from universal robots</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="thomas.timm.dk@gmail.com">Thomas Timm Andersen</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>Beerware</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">http://wiki.ros.org/ur_modern_driver</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<author email="thomas.timm.dk@gmail.com">Thomas Timm Andersen</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>actionlib</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>ur_msgs</build_depend>
<run_depend>actionlib</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>ur_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

392
src/robot_state_RT.cpp Normal file
View File

@@ -0,0 +1,392 @@
/*
* robotStateRT.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
* ----------------------------------------------------------------------------
*/
#include "ur_modern_driver/robot_state_RT.h"
RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
version_ = 0.0;
time_ = 0.0;
q_target_.assign(6, 0.0);
qd_target_.assign(6, 0.0);
qdd_target_.assign(6, 0.0);
i_target_.assign(6, 0.0);
m_target_.assign(6, 0.0);
q_actual_.assign(6, 0.0);
qd_actual_.assign(6, 0.0);
i_actual_.assign(6, 0.0);
i_control_.assign(6, 0.0);
tool_vector_actual_.assign(6, 0.0);
tcp_speed_actual_.assign(6, 0.0);
tcp_force_.assign(6, 0.0);
tool_vector_target_.assign(6, 0.0);
tcp_speed_target_.assign(6, 0.0);
digital_input_bits_.assign(64, false);
motor_temperatures_.assign(6, 0.0);
controller_timer_ = 0.0;
robot_mode_ = 0.0;
joint_modes_.assign(6, 0.0);
safety_mode_ = 0.0;
tool_accelerometer_values_.assign(3, 0.0);
speed_scaling_ = 0.0;
linear_momentum_norm_ = 0.0;
v_main_ = 0.0;
v_robot_ = 0.0;
i_robot_ = 0.0;
v_actual_.assign(6, 0.0);
new_data_available_ = false;
pMsg_cond_ = &msg_cond;
}
bool RobotStateRT::getNewDataAvailable() {
return new_data_available_;
}
void RobotStateRT::finishedReading() {
new_data_available_ = false;
}
double RobotStateRT::ntohd(uint64_t nf) {
double x;
nf = be64toh(nf);
memcpy(&x, &nf, sizeof(x));
return x;
}
std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
int nr_of_vals) {
uint64_t q;
std::vector<double> ret;
for (int i = 0; i < nr_of_vals; i++) {
memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
ret.push_back(ntohd(q));
}
return ret;
}
std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
std::vector<bool> ret;
for (int i = 0; i < 64; i++) {
ret.push_back((data & (1 << i)) >> i);
}
return ret;
}
double RobotStateRT::getVersion() {
double ret;
val_lock_.lock();
ret = version_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getTime() {
double ret;
val_lock_.lock();
ret = time_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = q_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQdTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = q_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQddTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = qdd_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getITarget() {
std::vector<double> ret;
val_lock_.lock();
ret = i_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getMTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = m_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQActual() {
std::vector<double> ret;
val_lock_.lock();
ret = q_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getQdActual() {
std::vector<double> ret;
val_lock_.lock();
ret = qd_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getIActual() {
std::vector<double> ret;
val_lock_.lock();
ret = i_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getIControl() {
std::vector<double> ret;
val_lock_.lock();
ret = i_control_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolVectorActual() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpSpeedActual() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_actual_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpForce() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_force_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolVectorTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_vector_target_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getTcpSpeedTarget() {
std::vector<double> ret;
val_lock_.lock();
ret = tcp_speed_target_;
val_lock_.unlock();
return ret;
}
std::vector<bool> RobotStateRT::getDigitalInputBits() {
std::vector<bool> ret;
val_lock_.lock();
ret = digital_input_bits_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getMotorTemperatures() {
std::vector<double> ret;
val_lock_.lock();
ret = motor_temperatures_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getControllerTimer() {
double ret;
val_lock_.lock();
ret = controller_timer_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getRobotMode() {
double ret;
val_lock_.lock();
ret = robot_mode_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getJointModes() {
std::vector<double> ret;
val_lock_.lock();
ret = joint_modes_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getSafety_mode() {
double ret;
val_lock_.lock();
ret = safety_mode_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getToolAccelerometerValues() {
std::vector<double> ret;
val_lock_.lock();
ret = tool_accelerometer_values_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getSpeedScaling() {
double ret;
val_lock_.lock();
ret = speed_scaling_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getLinearMomentumNorm() {
double ret;
val_lock_.lock();
ret = linear_momentum_norm_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getVMain() {
double ret;
val_lock_.lock();
ret = v_main_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getVRobot() {
double ret;
val_lock_.lock();
ret = v_robot_;
val_lock_.unlock();
return ret;
}
double RobotStateRT::getIRobot() {
double ret;
val_lock_.lock();
ret = i_robot_;
val_lock_.unlock();
return ret;
}
std::vector<double> RobotStateRT::getVActual() {
std::vector<double> ret;
val_lock_.lock();
ret = v_actual_;
val_lock_.unlock();
return ret;
}
void RobotStateRT::unpack(uint8_t * buf) {
int64_t digital_input_bits;
uint64_t unpack_to;
uint16_t offset = 0;
val_lock_.lock();
if (version_ == 0.0) {
uint32_t len;
memcpy(&len, &buf[offset], sizeof(len));
if (len <= 756) {
version_ = 1.6;
} else if (len <= 764) {
version_ = 1.7;
} else if (len <= 812) {
version_ = 1.8;
} else if (len <= 1044) {
version_ = 3.0;
}
}
offset += 4;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
time_ = ntohd(unpack_to);
offset += sizeof(double);
q_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qdd_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
m_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
q_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
qd_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
i_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
if (version_ <= 1.8) {
if (version_ != 1.6)
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * (3 + 15);
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
} else {
i_control_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_actual_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_force_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tool_vector_target_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
tcp_speed_target_ = unpackVector(buf, offset, 6);
}
offset += sizeof(double) * 6;
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
offset += sizeof(double);
motor_temperatures_ = unpackVector(buf, offset, 6);
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
controller_timer_ = ntohd(unpack_to);
if (version_ > 1.6) {
offset += sizeof(double) * 2;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
robot_mode_ = ntohd(unpack_to);
if (version_ > 1.7) {
offset += sizeof(double);
joint_modes_ = unpackVector(buf, offset, 6);
}
}
if (version_ > 1.8) {
offset += sizeof(double) * 6;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
safety_mode_ = ntohd(unpack_to);
offset += sizeof(double);
tool_accelerometer_values_ = unpackVector(buf, offset, 3);
offset += sizeof(double) * 3;
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
speed_scaling_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
linear_momentum_norm_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_main_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
v_robot_ = ntohd(unpack_to);
offset += sizeof(double);
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
i_robot_ = ntohd(unpack_to);
offset += sizeof(double);
v_actual_ = unpackVector(buf, offset, 6);
}
val_lock_.unlock();
new_data_available_ = true;
pMsg_cond_->notify_all();
}

33
src/ur_driver.cpp Normal file
View File

@@ -0,0 +1,33 @@
/*
* ur_driver.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
* ----------------------------------------------------------------------------
*/
#include "ur_modern_driver/ur_driver.h"
UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
uint safety_count_max) {
rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
safety_count_max);
}
void UrDriver::start() {
rt_interface_->start();
}
void UrDriver::halt() {
rt_interface_->halt();
}
void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
double q5, double acc) {
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
}

107
src/ur_realtime_communication.cpp Normal file
View File

@@ -0,0 +1,107 @@
/*
* ur_realtime_communication.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
* ----------------------------------------------------------------------------
*/
#include "ur_modern_driver/ur_realtime_communication.h"
UrRealtimeCommunication::UrRealtimeCommunication(
std::condition_variable& msg_cond, std::string host,
uint safety_count_max) :
SAMPLETIME_(0.008) {
robot_state_ = new RobotStateRT(msg_cond);
bzero((char *) &serv_addr_, sizeof(serv_addr_));
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd_ < 0) {
printf("ERROR opening socket");
exit(0);
}
server_ = gethostbyname(host.c_str());
if (server_ == NULL) {
printf("ERROR, no such host\n");
exit(0);
}
serv_addr_.sin_family = AF_INET;
bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
serv_addr_.sin_port = htons(30003);
flag_ = 1;
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
connected_ = false;
keepalive_ = false;
safety_count_ = 0;
safety_count_max_ = safety_count_max;
}
void UrRealtimeCommunication::start() {
keepalive_ = true;
if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
< 0)
printf("Error connecting");
printf("connecting...\n");
comThread_ = std::thread(&UrRealtimeCommunication::run, this);
}
void UrRealtimeCommunication::halt() {
keepalive_ = false;
comThread_.join();
}
void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
if (inp.back() != '\n') {
inp.append("\n");
}
command_string_lock_.lock();
command_ += inp;
command_string_lock_.unlock();
}
void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
double q3, double q4, double q5, double acc) {
char cmd[1024];
sprintf(cmd,
"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %1.5f,0.02)\n",
q0, q1, q2, q3, q4, q5, acc);
addCommandToQueue((std::string) (cmd));
if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
//If a joint speed is set, make sure we stop it again after some time if the user doesn't
safety_count_ = 0;
}
}
void UrRealtimeCommunication::run() {
uint8_t buf[2048];
bzero(buf, 2048);
printf("Got connection\n");
connected_ = true;
while (keepalive_) {
read(sockfd_, buf, 2048);
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
sizeof(int));
robot_state_->unpack(buf);
command_string_lock_.lock();
if (command_.length() != 0) {
write(sockfd_, command_.c_str(), command_.length());
command_ = "";
}
if (safety_count_ == safety_count_max_) {
setSpeed(0., 0., 0., 0., 0., 0.);
write(sockfd_, command_.c_str(), command_.length());
command_ = "";
}
safety_count_ += 1;
command_string_lock_.unlock();
}
}
void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
safety_count_max_ = inp;
}

93
src/ur_ros_wrapper.cpp Normal file
View File

@@ -0,0 +1,93 @@
/*
* ur_driver.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
* can do whatever you want with this stuff. If we meet some day, and you think
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
* ----------------------------------------------------------------------------
*/
#include "ros/ros.h"
#include <ros/console.h>
#include "ur_modern_driver/ur_driver.h"
#include <string.h>
#include <vector>
#include <mutex>
#include <condition_variable>
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/WrenchStamped.h"
#include <thread>
std::condition_variable g_msg_cond;
void publishRTMsg(UrDriver robot, std::vector<std::string> joint_names) {
ros::NodeHandle n_rt;
ros::Publisher joint_pub = n_rt.advertise<sensor_msgs::JointState>("/joint_states", 1);
ros::Publisher wrench_pub = n_rt.advertise<geometry_msgs::WrenchStamped>("/wrench", 1);
while (ros::ok()) {
sensor_msgs::JointState joint_msg;
joint_msg.name = joint_names;
geometry_msgs::WrenchStamped wrench_msg;
std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
std::unique_lock<std::mutex> locker(msg_lock);
while (!robot.rt_interface_->robot_state_->getNewDataAvailable()) {
g_msg_cond.wait(locker);
}
joint_msg.header.stamp = ros::Time::now();
joint_msg.position = robot.rt_interface_->robot_state_->getQActual();
joint_msg.velocity = robot.rt_interface_->robot_state_->getQdActual();
joint_msg.effort = robot.rt_interface_->robot_state_->getIActual();
joint_pub.publish(joint_msg);
std::vector<double> tcp_force = robot.rt_interface_->robot_state_->getTcpForce();
wrench_msg.header.stamp = joint_msg.header.stamp;
wrench_msg.wrench.force.x = tcp_force[0];
wrench_msg.wrench.force.y = tcp_force[1];
wrench_msg.wrench.force.z = tcp_force[2];
wrench_msg.wrench.torque.x = tcp_force[3];
wrench_msg.wrench.torque.y = tcp_force[4];
wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg);
robot.rt_interface_->robot_state_->finishedReading();
}
}
int main(int argc, char **argv) {
bool use_sim_time = false;
std::string joint_prefix ="";
std::string host;
std::vector<std::string> joint_names;
UrDriver robot(g_msg_cond, host);
ros::init(argc, argv, "ur_driver");
ros::NodeHandle n;
if (ros::param::get("use_sim_time", use_sim_time)) {
ROS_WARN("use_sim_time is set!!");
}
if (ros::param::get("~prefix", joint_prefix)) {
ROS_INFO("Setting prefix to %s", joint_prefix.c_str());
}
joint_names.push_back(joint_prefix + "shoulder_pan_joint");
joint_names.push_back(joint_prefix + "should_lift_joint");
joint_names.push_back(joint_prefix + "elbow_joint");
joint_names.push_back(joint_prefix + "wrist_1_joint");
joint_names.push_back(joint_prefix + "wrist_2_joint");
joint_names.push_back(joint_prefix + "wrist_3_joint");
if (argc > 1) {
host = argv[1];
} else if (!(ros::param::get("~robot_ip", host))) {
ROS_FATAL("Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
exit(1);
}
robot.start();
std::thread rt_publish_thread(publishRTMsg, robot, joint_names);
ros::spin();
robot.halt();
exit(0);
}