diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..5b0ed84 --- /dev/null +++ b/CMakeLists.txt @@ -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) diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h new file mode 100644 index 0000000..5851fff --- /dev/null +++ b/include/ur_modern_driver/robot_state_RT.h @@ -0,0 +1,101 @@ +/* + * robotStateRT.h + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * 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 +#include +#include +#include +#include +#include + +class RobotStateRT { +private: + double version_; //protocol version + + double time_; //Time elapsed since the controller was started + std::vector q_target_; //Target joint positions + std::vector qd_target_; //Target joint velocities + std::vector qdd_target_; //Target joint accelerations + std::vector i_target_; //Target joint currents + std::vector m_target_; //Target joint moments (torques) + std::vector q_actual_; //Actual joint positions + std::vector qd_actual_; //Actual joint velocities + std::vector i_actual_; //Actual joint currents + std::vector i_control_; //Joint control currents + std::vector 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 tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates + std::vector tcp_force_; //Generalised forces in the TC + std::vector 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 tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates + std::vector 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 motor_temperatures_; //Temperature of each joint in degrees celsius + double controller_timer_; //Controller realtime thread execution time + double robot_mode_; //Robot mode + std::vector joint_modes_; //Joint control modes + double safety_mode_; //Safety mode + std::vector 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 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 unpackVector(uint8_t * buf, int start_index, + int nr_of_vals); + std::vector unpackDigitalInputBits(int64_t data); + double ntohd(uint64_t nf); + +public: + RobotStateRT(std::condition_variable& msg_cond); + double getVersion(); + double getTime(); + std::vector getQTarget(); + std::vector getQdTarget(); + std::vector getQddTarget(); + std::vector getITarget(); + std::vector getMTarget(); + std::vector getQActual(); + std::vector getQdActual(); + std::vector getIActual(); + std::vector getIControl(); + std::vector getToolVectorActual(); + std::vector getTcpSpeedActual(); + std::vector getTcpForce(); + std::vector getToolVectorTarget(); + std::vector getTcpSpeedTarget(); + std::vector getDigitalInputBits(); + std::vector getMotorTemperatures(); + double getControllerTimer(); + double getRobotMode(); + std::vector getJointModes(); + double getSafety_mode(); + std::vector getToolAccelerometerValues(); + double getSpeedScaling(); + double getLinearMomentumNorm(); + double getVMain(); + double getVRobot(); + double getIRobot(); + bool getNewDataAvailable(); + void finishedReading(); + std::vector getVActual(); + void unpack(uint8_t * buf); +}; + +#endif /* ROBOTSTATERT_H_ */ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h new file mode 100644 index 0000000..615b17c --- /dev/null +++ b/include/ur_modern_driver/ur_driver.h @@ -0,0 +1,32 @@ +/* + * ur_driver + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * 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 +#include +#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_ */ diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h new file mode 100644 index 0000000..eecdd66 --- /dev/null +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -0,0 +1,62 @@ +/* + * ur_realtime_communication.h + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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_ */ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..36a3ac0 --- /dev/null +++ b/package.xml @@ -0,0 +1,64 @@ + + + ur_modern_driver + 0.0.1 + The new driver for the UR3/UR5/UR10 robot arms from universal robots + + + + + Thomas Timm Andersen + + + + + + Beerware + + + + + + http://wiki.ros.org/ur_modern_driver + + + + + + Thomas Timm Andersen + + + + + + + + + + + + + + catkin + actionlib + control_msgs + geometry_msgs + roscpp + sensor_msgs + trajectory_msgs + ur_msgs + actionlib + control_msgs + geometry_msgs + roscpp + sensor_msgs + trajectory_msgs + ur_msgs + + + + + + + + diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp new file mode 100644 index 0000000..3233c85 --- /dev/null +++ b/src/robot_state_RT.cpp @@ -0,0 +1,392 @@ +/* + * robotStateRT.cpp + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * 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 RobotStateRT::unpackVector(uint8_t * buf, int start_index, + int nr_of_vals) { + uint64_t q; + std::vector 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 RobotStateRT::unpackDigitalInputBits(int64_t data) { + std::vector 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 RobotStateRT::getQTarget() { + std::vector ret; + val_lock_.lock(); + ret = q_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQdTarget() { + std::vector ret; + val_lock_.lock(); + ret = q_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQddTarget() { + std::vector ret; + val_lock_.lock(); + ret = qdd_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getITarget() { + std::vector ret; + val_lock_.lock(); + ret = i_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getMTarget() { + std::vector ret; + val_lock_.lock(); + ret = m_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQActual() { + std::vector ret; + val_lock_.lock(); + ret = q_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getQdActual() { + std::vector ret; + val_lock_.lock(); + ret = qd_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getIActual() { + std::vector ret; + val_lock_.lock(); + ret = i_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getIControl() { + std::vector ret; + val_lock_.lock(); + ret = i_control_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getToolVectorActual() { + std::vector ret; + val_lock_.lock(); + ret = tool_vector_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getTcpSpeedActual() { + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_actual_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getTcpForce() { + std::vector ret; + val_lock_.lock(); + ret = tcp_force_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getToolVectorTarget() { + std::vector ret; + val_lock_.lock(); + ret = tool_vector_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getTcpSpeedTarget() { + std::vector ret; + val_lock_.lock(); + ret = tcp_speed_target_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getDigitalInputBits() { + std::vector ret; + val_lock_.lock(); + ret = digital_input_bits_; + val_lock_.unlock(); + return ret; +} +std::vector RobotStateRT::getMotorTemperatures() { + std::vector 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 RobotStateRT::getJointModes() { + std::vector 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 RobotStateRT::getToolAccelerometerValues() { + std::vector 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 RobotStateRT::getVActual() { + std::vector 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(); + +} + diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp new file mode 100644 index 0000000..5856c64 --- /dev/null +++ b/src/ur_driver.cpp @@ -0,0 +1,33 @@ +/* + * ur_driver.cpp + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * 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); +} + diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp new file mode 100644 index 0000000..b5dd82b --- /dev/null +++ b/src/ur_realtime_communication.cpp @@ -0,0 +1,107 @@ +/* + * ur_realtime_communication.cpp + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * 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; +} + diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp new file mode 100644 index 0000000..bb9e024 --- /dev/null +++ b/src/ur_ros_wrapper.cpp @@ -0,0 +1,93 @@ +/* + * ur_driver.cpp + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * 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 +#include "ur_modern_driver/ur_driver.h" +#include +#include +#include +#include +#include "sensor_msgs/JointState.h" +#include "geometry_msgs/WrenchStamped.h" +#include "ur_msgs/RobotStateRTMsg.h" + +UrDriver* g_robot; +std::mutex g_msg_lock; +std::condition_variable g_msg_cond; +std::vector g_joint_names; + +void publishRTMsg() { + ros::NodeHandle n_rt; + ros::Publisher joint_pub = n_rt.advertise("/joint_states", 1); + ros::Publisher wrench_pub = n_rt.advertise("/wrench", 1); + while (ros::ok()) { + sensor_msgs::JointState joint_msg; + joint_msg.name = g_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 locker(msg_lock); + while (!g_robot->rt_interface_->robot_state_->getNewDataAvailable()) { + g_msg_cond.wait(locker); + } + joint_msg.header.stamp = ros::Time::now(); + joint_msg.position = g_robot->rt_interface_->robot_state_->getQActual(); + joint_msg.velocity = g_robot->rt_interface_->robot_state_->getQdActual(); + joint_msg.effort = g_robot->rt_interface_->robot_state_->getIActual(); + joint_pub.publish(joint_msg); + std::vector tcp_force = g_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); + + g_robot->rt_interface_->robot_state_->finishedReading(); + + } +} + +int main(int argc, char **argv) { + bool use_sim_time = false; + std::string joint_prefix =""; + std::string 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()); + } + g_joint_names.push_back(joint_prefix + "shoulder_pan_joint"); + g_joint_names.push_back(joint_prefix + "should_lift_joint"); + g_joint_names.push_back(joint_prefix + "elbow_joint"); + g_joint_names.push_back(joint_prefix + "wrist_1_joint"); + g_joint_names.push_back(joint_prefix + "wrist_2_joint"); + g_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); + } + g_robot = new UrDriver(g_msg_cond, host); + g_robot->start(); + publishRTMsg(); + ros::spin(); + g_robot->halt(); + exit(0); +}