From fdaaacfe2c08e8a0f3ec6f296949075a04ffc081 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 02:45:02 +0200 Subject: [PATCH] Clean up of old driver files --- CMakeLists.txt | 16 +- include/ur_modern_driver/packet.h | 8 - include/ur_modern_driver/parser.h | 9 - include/ur_modern_driver/robot_state.h | 233 ----- include/ur_modern_driver/robot_state_RT.h | 119 --- include/ur_modern_driver/ros/robot_hardware.h | 53 -- include/ur_modern_driver/ur_communication.h | 63 -- include/ur_modern_driver/ur_driver.h | 97 -- .../ur_modern_driver/ur_hardware_interface.h | 136 --- .../ur_realtime_communication.h | 73 -- src/do_output.cpp | 62 -- src/ros/robot_hardware.cpp | 30 - src/ur_communication.cpp | 184 ---- src/ur_driver.cpp | 420 --------- src/ur_hardware_interface.cpp | 283 ------ src/ur_realtime_communication.cpp | 211 ----- src/ur_ros_wrapper.cpp | 871 ------------------ 17 files changed, 5 insertions(+), 2863 deletions(-) delete mode 100644 include/ur_modern_driver/packet.h delete mode 100644 include/ur_modern_driver/parser.h delete mode 100644 include/ur_modern_driver/robot_state.h delete mode 100644 include/ur_modern_driver/robot_state_RT.h delete mode 100644 include/ur_modern_driver/ros/robot_hardware.h delete mode 100644 include/ur_modern_driver/ur_communication.h delete mode 100644 include/ur_modern_driver/ur_driver.h delete mode 100644 include/ur_modern_driver/ur_hardware_interface.h delete mode 100644 include/ur_modern_driver/ur_realtime_communication.h delete mode 100644 src/do_output.cpp delete mode 100644 src/ros/robot_hardware.cpp delete mode 100644 src/ur_communication.cpp delete mode 100644 src/ur_driver.cpp delete mode 100644 src/ur_hardware_interface.cpp delete mode 100644 src/ur_realtime_communication.cpp delete mode 100644 src/ur_ros_wrapper.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index be59d00..a950358 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,7 +134,7 @@ else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ") endif() -set(CMAKE_CXX_FLAGS "-g -Wall -Wextra ${CMAKE_CXX_FLAGS}") +set(CMAKE_CXX_FLAGS "-g -Wall -Wextra -Wno-unused-parameter ${CMAKE_CXX_FLAGS}") ## Specify additional locations of header files ## Your package locations should be listed before other locations @@ -146,7 +146,9 @@ include_directories(include ## Declare a C++ library # Hardware Interface -add_library(ur_hardware_interface src/ur_hardware_interface.cpp) +add_library(ur_hardware_interface + src/ros/hardware_interface.cpp + src/ros/controller.cpp) target_link_libraries(ur_hardware_interface ${catkin_LIBRARIES} ) @@ -159,8 +161,6 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES src/ros/action_server.cpp - src/ros/controller.cpp - src/ros/hardware_interface.cpp src/ros/mb_publisher.cpp src/ros/rt_publisher.cpp src/ros/service_stopper.cpp @@ -172,13 +172,7 @@ set(${PROJECT_NAME}_SOURCES src/ur/master_board.cpp src/ur/rt_state.cpp src/ur/messages.cpp - src/tcp_socket.cpp - src/ur_driver.cpp - src/ur_realtime_communication.cpp - src/ur_communication.cpp - src/robot_state.cpp - src/robot_state_RT.cpp - src/do_output.cpp) + src/tcp_socket.cpp) add_executable(ur_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp) diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h deleted file mode 100644 index 521d75b..0000000 --- a/include/ur_modern_driver/packet.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once -#include "ur_modern_driver/bin_parser.h" - -class Packet -{ -public: - virtual bool parseWith(BinParser& bp) = 0; -}; \ No newline at end of file diff --git a/include/ur_modern_driver/parser.h b/include/ur_modern_driver/parser.h deleted file mode 100644 index e824c57..0000000 --- a/include/ur_modern_driver/parser.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once -#include "ur_modern_driver/bin_parser.h" -#include "ur_modern_driver/packet.h" - -class Parser -{ -public: - virtual std::unique_ptr parse(BinParser& bp) = 0; -}; diff --git a/include/ur_modern_driver/robot_state.h b/include/ur_modern_driver/robot_state.h deleted file mode 100644 index 621926a..0000000 --- a/include/ur_modern_driver/robot_state.h +++ /dev/null @@ -1,233 +0,0 @@ -/* - * robot_state.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ROBOT_STATE_H_ -#define ROBOT_STATE_H_ - -#include -#include -#include -#include -#include -#include -#include - -namespace message_types -{ -enum message_type -{ - ROBOT_STATE = 16, - ROBOT_MESSAGE = 20, - PROGRAM_STATE_MESSAGE = 25 -}; -} -typedef message_types::message_type messageType; - -namespace package_types -{ -enum package_type -{ - ROBOT_MODE_DATA = 0, - JOINT_DATA = 1, - TOOL_DATA = 2, - MASTERBOARD_DATA = 3, - CARTESIAN_INFO = 4, - KINEMATICS_INFO = 5, - CONFIGURATION_DATA = 6, - FORCE_MODE_DATA = 7, - ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 -}; -} -typedef package_types::package_type packageType; - -namespace robot_message_types -{ -enum robot_message_type -{ - ROBOT_MESSAGE_TEXT = 0, - ROBOT_MESSAGE_PROGRAM_LABEL = 1, - PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2, - ROBOT_MESSAGE_VERSION = 3, - ROBOT_MESSAGE_SAFETY_MODE = 5, - ROBOT_MESSAGE_ERROR_CODE = 6, - ROBOT_MESSAGE_KEY = 7, - ROBOT_MESSAGE_REQUEST_VALUE = 9, - ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10 -}; -} -typedef robot_message_types::robot_message_type robotMessageType; - -namespace robot_state_type_v18 -{ -enum robot_state_type -{ - ROBOT_RUNNING_MODE = 0, - ROBOT_FREEDRIVE_MODE = 1, - ROBOT_READY_MODE = 2, - ROBOT_INITIALIZING_MODE = 3, - ROBOT_SECURITY_STOPPED_MODE = 4, - ROBOT_EMERGENCY_STOPPED_MODE = 5, - ROBOT_FATAL_ERROR_MODE = 6, - ROBOT_NO_POWER_MODE = 7, - ROBOT_NOT_CONNECTED_MODE = 8, - ROBOT_SHUTDOWN_MODE = 9, - ROBOT_SAFEGUARD_STOP_MODE = 10 -}; -} -typedef robot_state_type_v18::robot_state_type robotStateTypeV18; -namespace robot_state_type_v30 -{ -enum robot_state_type -{ - ROBOT_MODE_DISCONNECTED = 0, - ROBOT_MODE_CONFIRM_SAFETY = 1, - ROBOT_MODE_BOOTING = 2, - ROBOT_MODE_POWER_OFF = 3, - ROBOT_MODE_POWER_ON = 4, - ROBOT_MODE_IDLE = 5, - ROBOT_MODE_BACKDRIVE = 6, - ROBOT_MODE_RUNNING = 7, - ROBOT_MODE_UPDATING_FIRMWARE = 8 -}; -} - -typedef robot_state_type_v30::robot_state_type robotStateTypeV30; - -struct version_message -{ - uint64_t timestamp; - int8_t source; - int8_t robot_message_type; - int8_t project_name_size; - char project_name[15]; - uint8_t major_version; - uint8_t minor_version; - int svn_revision; - char build_date[25]; -}; - -struct masterboard_data -{ - int digitalInputBits; - int digitalOutputBits; - char analogInputRange0; - char analogInputRange1; - double analogInput0; - double analogInput1; - char analogOutputDomain0; - char analogOutputDomain1; - double analogOutput0; - double analogOutput1; - float masterBoardTemperature; - float robotVoltage48V; - float robotCurrent; - float masterIOCurrent; - unsigned char safetyMode; - unsigned char masterOnOffState; - char euromap67InterfaceInstalled; - int euromapInputBits; - int euromapOutputBits; - float euromapVoltage; - float euromapCurrent; -}; - -struct robot_mode_data -{ - uint64_t timestamp; - bool isRobotConnected; - bool isRealRobotEnabled; - bool isPowerOnRobot; - bool isEmergencyStopped; - bool isProtectiveStopped; - bool isProgramRunning; - bool isProgramPaused; - unsigned char robotMode; - unsigned char controlMode; - double targetSpeedFraction; - double speedScaling; -}; - -class RobotState -{ -private: - version_message version_msg_; - masterboard_data mb_data_; - robot_mode_data robot_mode_; - - std::recursive_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 - unsigned char robot_mode_running_; - - double ntohd(uint64_t nf); - -public: - RobotState(std::condition_variable& msg_cond); - ~RobotState(); - double getVersion(); - double getTime(); - std::vector getQTarget(); - int getDigitalInputBits(); - int getDigitalOutputBits(); - char getAnalogInputRange0(); - char getAnalogInputRange1(); - double getAnalogInput0(); - double getAnalogInput1(); - char getAnalogOutputDomain0(); - char getAnalogOutputDomain1(); - double getAnalogOutput0(); - double getAnalogOutput1(); - std::vector getVActual(); - float getMasterBoardTemperature(); - float getRobotVoltage48V(); - float getRobotCurrent(); - float getMasterIOCurrent(); - unsigned char getSafetyMode(); - unsigned char getInReducedMode(); - char getEuromap67InterfaceInstalled(); - int getEuromapInputBits(); - int getEuromapOutputBits(); - float getEuromapVoltage(); - float getEuromapCurrent(); - - bool isRobotConnected(); - bool isRealRobotEnabled(); - bool isPowerOnRobot(); - bool isEmergencyStopped(); - bool isProtectiveStopped(); - bool isProgramRunning(); - bool isProgramPaused(); - unsigned char getRobotMode(); - bool isReady(); - - void setDisconnected(); - - bool getNewDataAvailable(); - void finishedReading(); - - void unpack(uint8_t* buf, unsigned int buf_length); - void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len); - void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset); - void unpackRobotMode(uint8_t* buf, unsigned int offset); -}; - -#endif /* ROBOT_STATE_H_ */ diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h deleted file mode 100644 index bca6fac..0000000 --- a/include/ur_modern_driver/robot_state_RT.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * robotStateRT.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ROBOT_STATE_RT_H_ -#define ROBOT_STATE_RT_H_ - -#include -#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 data_published_; // to avoid spurious wakes - bool controller_updated_; // 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); - ~RobotStateRT(); - 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(); - - void setVersion(double ver); - - void setDataPublished(); - bool getDataPublished(); - bool getControllerUpdated(); - void setControllerUpdated(); - std::vector getVActual(); - void unpack(uint8_t* buf); -}; - -#endif /* ROBOT_STATE_RT_H_ */ diff --git a/include/ur_modern_driver/ros/robot_hardware.h b/include/ur_modern_driver/ros/robot_hardware.h deleted file mode 100644 index 47ef455..0000000 --- a/include/ur_modern_driver/ros/robot_hardware.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include -#include "ur_modern_driver/ros/hardware_interface.h" - -using hardware_interface::RobotHW; -using hardware_interface::ControllerInfo; - -class RobotHardware : public RobotHW -{ -private: - JointInterface joint_interface_; - WrenchInterface wrench_interface_; - PositionInterface position_interface_; - VelocityInterface velocity_interface_; - - HardwareInterface* active_interface_; - std::map available_interfaces_; - - template - void registerHardwareInterface(T* interface) - { - registerInterface(interface); - available_interfaces_[hardware_interface::internal::demangledTypeName()] = interface; - } - -public: - RobotHardware(URCommander& commander, std::vector& joint_names, double max_vel_change) - : joint_interface_(joint_names) - , wrench_interface_() - , position_interface_(commander, joint_interface_, joint_names) - , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) - { - registerInterface(&joint_interface_); - registerInterface(&wrench_interface_); - - registerHardwareInterface(&position_interface_); - registerHardwareInterface(&velocity_interface_); - } - - // bool canSwitch(const std::list& start_list, const std::list& stop_list) const; - void doSwitch(const std::list& start_list, const std::list& stop_list); - - /// \brief Read the state from the robot hardware. - virtual void read(RTShared& packet); - - /// \brief write the command to the robot hardware. - virtual void write(); -}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h deleted file mode 100644 index 29460c5..0000000 --- a/include/ur_modern_driver/ur_communication.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * ur_communication.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef UR_COMMUNICATION_H_ -#define UR_COMMUNICATION_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "robot_state.h" - -class UrCommunication -{ -private: - int pri_sockfd_, sec_sockfd_; - struct sockaddr_in pri_serv_addr_, sec_serv_addr_; - struct hostent* server_; - bool keepalive_; - std::thread comThread_; - int flag_; - void run(); - -public: - bool connected_; - RobotState* robot_state_; - - UrCommunication(std::condition_variable& msg_cond, std::string host); - bool start(); - void halt(); -}; - -#endif /* UR_COMMUNICATION_H_ */ diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h deleted file mode 100644 index dbfb3bd..0000000 --- a/include/ur_modern_driver/ur_driver.h +++ /dev/null @@ -1,97 +0,0 @@ -/* - * ur_driver - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef UR_DRIVER_H_ -#define UR_DRIVER_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "ur_communication.h" -#include "ur_realtime_communication.h" - -#include - -class UrDriver -{ -private: - double maximum_time_step_; - double minimum_payload_; - double maximum_payload_; - std::vector joint_names_; - std::string ip_addr_; - const int MULT_JOINTSTATE_ = 1000000; - const int MULT_TIME_ = 1000000; - const unsigned int REVERSE_PORT_; - int incoming_sockfd_; - int new_sockfd_; - bool reverse_connected_; - double servoj_time_; - bool executing_traj_; - double firmware_version_; - double servoj_lookahead_time_; - double servoj_gain_; - -public: - UrRealtimeCommunication* rt_interface_; - UrCommunication* sec_interface_; - - UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, - double max_time_step = 0.08, double min_payload = 0., double max_payload = 1., - double servoj_lookahead_time = 0.03, double servoj_gain = 300.); - bool start(); - void halt(); - - void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - - bool doTraj(std::vector inp_timestamps, std::vector> inp_positions, - std::vector> inp_velocities); - void servoj(std::vector positions, int keepalive = 1); - - void stopTraj(); - - bool uploadProg(); - bool openServo(); - void closeServo(std::vector positions); - - std::vector interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel); - - std::vector getJointNames(); - void setJointNames(std::vector jn); - void setToolVoltage(unsigned int v); - void setFlag(unsigned int n, bool b); - void setDigitalOut(unsigned int n, bool b); - void setAnalogOut(unsigned int n, double f); - bool setPayload(double m); - - void setMinPayload(double m); - void setMaxPayload(double m); - void setServojTime(double t); - void setServojLookahead(double t); - void setServojGain(double g); -}; - -#endif /* UR_DRIVER_H_ */ diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h deleted file mode 100644 index b2a7db1..0000000 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ /dev/null @@ -1,136 +0,0 @@ -/* - * ur_hardware_control_loop.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* Based on original source from University of Colorado, Boulder. License copied below. */ - -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - ********************************************************************* - - Author: Dave Coleman - */ - -#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H -#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "ur_driver.h" - -namespace ros_control_ur -{ -// For simulation only - determines how fast a trajectory is followed -static const double POSITION_STEP_FACTOR = 1; -static const double VELOCITY_STEP_FACTOR = 1; - -/// \brief Hardware interface for a robot -class UrHardwareInterface : public hardware_interface::RobotHW -{ -public: - /** - * \brief Constructor - * \param nh - Node handle for topics. - */ - UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); - - /// \brief Initialize the hardware interface - virtual void init(); - - /// \brief Read the state from the robot hardware. - virtual void read(); - - /// \brief write the command to the robot hardware. - virtual void write(); - - void setMaxVelChange(double inp); - - bool canSwitch(const std::list& start_list, - const std::list& stop_list) const; - void doSwitch(const std::list& start_list, - const std::list& stop_list); - -protected: - // Startup and shutdown of the internal node inside a roscpp program - ros::NodeHandle nh_; - - // Interfaces - hardware_interface::JointStateInterface joint_state_interface_; - hardware_interface::ForceTorqueSensorInterface force_torque_interface_; - hardware_interface::PositionJointInterface position_joint_interface_; - hardware_interface::VelocityJointInterface velocity_joint_interface_; - bool velocity_interface_running_; - bool position_interface_running_; - // Shared memory - std::vector joint_names_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - std::vector prev_joint_velocity_command_; - std::size_t num_joints_; - double robot_force_[3] = { 0., 0., 0. }; - double robot_torque_[3] = { 0., 0., 0. }; - - double max_vel_change_; - - // Robot API - UrDriver* robot_; -}; -// class - -} // namespace - -#endif diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h deleted file mode 100644 index 1de9fdb..0000000 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * ur_realtime_communication.h - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef UR_REALTIME_COMMUNICATION_H_ -#define UR_REALTIME_COMMUNICATION_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "do_output.h" -#include "robot_state_RT.h" - -class UrRealtimeCommunication -{ -private: - unsigned int safety_count_max_; - int sockfd_; - struct sockaddr_in serv_addr_; - struct hostent* server_; - std::string local_ip_; - bool keepalive_; - std::thread comThread_; - int flag_; - std::recursive_mutex command_string_lock_; - std::string command_; - unsigned int safety_count_; - void run(); - -public: - bool connected_; - RobotStateRT* robot_state_; - - UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12); - bool 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); - std::string getLocalIp(); -}; - -#endif /* UR_REALTIME_COMMUNICATION_H_ */ diff --git a/src/do_output.cpp b/src/do_output.cpp deleted file mode 100644 index 765e9b4..0000000 --- a/src/do_output.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/* - * do_output.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/do_output.h" - -void print_debug(std::string inp) -{ -#ifdef ROS_BUILD - ROS_DEBUG("%s", inp.c_str()); -#else - printf("DEBUG: %s\n", inp.c_str()); -#endif -} -void print_info(std::string inp) -{ -#ifdef ROS_BUILD - ROS_INFO("%s", inp.c_str()); -#else - printf("INFO: %s\n", inp.c_str()); -#endif -} -void print_warning(std::string inp) -{ -#ifdef ROS_BUILD - ROS_WARN("%s", inp.c_str()); -#else - printf("WARNING: %s\n", inp.c_str()); -#endif -} -void print_error(std::string inp) -{ -#ifdef ROS_BUILD - ROS_ERROR("%s", inp.c_str()); -#else - printf("ERROR: %s\n", inp.c_str()); -#endif -} -void print_fatal(std::string inp) -{ -#ifdef ROS_BUILD - ROS_FATAL("%s", inp.c_str()); - ros::shutdown(); -#else - printf("FATAL: %s\n", inp.c_str()); - exit(1); -#endif -} diff --git a/src/ros/robot_hardware.cpp b/src/ros/robot_hardware.cpp deleted file mode 100644 index 008daec..0000000 --- a/src/ros/robot_hardware.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "ur_modern_driver/ros/robot_hardware.h" - -/* -bool RobotHardware::canSwitch(const std::list& start_list, - const std::list& stop_list) const -{ - - bool running = active_interface_ != nullptr; - size_t start_size = start_list.size(); - size_t stop_size = stop_list.size(); - - - for (auto const& ci : stop_list) - { - auto it = interfaces_.find(ci.hardware_interface); - if(it == interfaces_.end() || it->second != active_interface_) - return false; - } - - for (auto const& ci : start_list) - { - auto it = interfaces_.find(ci.hardware_interface); - //we can only start a controller that's already running if we stop it first - if(it == interfaces_.end() || (it->second == active_interface_ && stop_size == 0)) - return false; - } - - return true; -} -*/ diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp deleted file mode 100644 index d7da04f..0000000 --- a/src/ur_communication.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/* - * ur_communication.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/ur_communication.h" - -UrCommunication::UrCommunication(std::condition_variable& msg_cond, std::string host) -{ - robot_state_ = new RobotState(msg_cond); - bzero((char*)&pri_serv_addr_, sizeof(pri_serv_addr_)); - bzero((char*)&sec_serv_addr_, sizeof(sec_serv_addr_)); - pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (pri_sockfd_ < 0) - { - print_fatal("ERROR opening socket pri_sockfd"); - } - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) - { - print_fatal("ERROR opening socket sec_sockfd"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) - { - print_fatal("ERROR, unknown host"); - } - pri_serv_addr_.sin_family = AF_INET; - sec_serv_addr_.sin_family = AF_INET; - bcopy((char*)server_->h_addr, (char*)&pri_serv_addr_.sin_addr.s_addr, server_->h_length); - bcopy((char*)server_->h_addr, (char*)&sec_serv_addr_.sin_addr.s_addr, server_->h_length); - pri_serv_addr_.sin_port = htons(30001); - sec_serv_addr_.sin_port = htons(30002); - flag_ = 1; - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(pri_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; -} - -bool UrCommunication::start() -{ - keepalive_ = true; - uint8_t buf[512]; - unsigned int bytes_read; - std::string cmd; - bzero(buf, 512); - print_debug("Acquire firmware version: Connecting..."); - if (connect(pri_sockfd_, (struct sockaddr*)&pri_serv_addr_, sizeof(pri_serv_addr_)) < 0) - { - print_fatal("Error connecting to get firmware version"); - return false; - } - print_debug("Acquire firmware version: Got connection"); - bytes_read = read(pri_sockfd_, buf, 512); - setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - // wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - char tmp[64]; - sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); - print_debug(tmp); - close(pri_sockfd_); - - print_debug("Switching to secondary interface for masterboard data: Connecting..."); - - fd_set writefds; - struct timeval timeout; - - connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_fatal("Error connecting to secondary interface"); - return false; - } - print_debug("Secondary interface: Got connection"); - comThread_ = std::thread(&UrCommunication::run, this); - return true; -} - -void UrCommunication::halt() -{ - keepalive_ = false; - comThread_.join(); -} - -void UrCommunication::run() -{ - uint8_t buf[2048]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sec_sockfd_, &readfds); - connected_ = true; - while (keepalive_) - { - while (connected_ && keepalive_) - { - timeout.tv_sec = 0; // do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes - if (bytes_read > 0) - { - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - robot_state_->unpack(buf, bytes_read); - } - else - { - connected_ = false; - robot_state_->setDisconnected(); - close(sec_sockfd_); - } - } - if (keepalive_) - { - // reconnect - print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sec_sockfd_ < 0) - { - print_fatal("ERROR opening secondary socket"); - } - flag_ = 1; - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) - { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; - - connect(sec_sockfd_, (struct sockaddr*)&sec_serv_addr_, sizeof(sec_serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sec_sockfd_, &writefds); - select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 " - "seconds..."); - } - else - { - connected_ = true; - print_info("Secondary port: Reconnected"); - } - } - } - } - - // wait for some traffic so the UR socket doesn't die in version 3.1. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - close(sec_sockfd_); -} diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp deleted file mode 100644 index c5cf3bd..0000000 --- a/src/ur_driver.cpp +++ /dev/null @@ -1,420 +0,0 @@ -/* - * ur_driver.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/ur_driver.h" - -UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step, - double min_payload, double max_payload, double servoj_lookahead_time, double servoj_gain) - : REVERSE_PORT_(reverse_port) - , maximum_time_step_(max_time_step) - , minimum_payload_(min_payload) - , maximum_payload_(max_payload) - , servoj_time_(servoj_time) - , servoj_lookahead_time_(servoj_lookahead_time) - , servoj_gain_(servoj_gain) -{ - char buffer[256]; - struct sockaddr_in serv_addr; - int n, flag; - - firmware_version_ = 0; - reverse_connected_ = false; - executing_traj_ = false; - rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); - new_sockfd_ = -1; - sec_interface_ = new UrCommunication(msg_cond, host); - - incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (incoming_sockfd_ < 0) - { - print_fatal("ERROR opening socket for reverse communication"); - } - bzero((char*)&serv_addr, sizeof(serv_addr)); - - serv_addr.sin_family = AF_INET; - serv_addr.sin_addr.s_addr = INADDR_ANY; - serv_addr.sin_port = htons(REVERSE_PORT_); - flag = 1; - setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int)); - setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); - if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) - { - print_fatal("ERROR on binding socket for reverse communication"); - } - listen(incoming_sockfd_, 5); -} - -std::vector UrDriver::interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, - std::vector p0_vel, std::vector p1_vel) -{ - /*Returns positions of the joints at time 't' */ - std::vector positions; - for (unsigned int i = 0; i < p0_pos.size(); i++) - { - double a = p0_pos[i]; - double b = p0_vel[i]; - double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2); - double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3); - positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); - } - return positions; -} - -bool UrDriver::doTraj(std::vector inp_timestamps, std::vector> inp_positions, - std::vector> inp_velocities) -{ - std::chrono::high_resolution_clock::time_point t0, t; - std::vector positions; - unsigned int j; - - if (!UrDriver::uploadProg()) - { - return false; - } - executing_traj_ = true; - t0 = std::chrono::high_resolution_clock::now(); - t = t0; - j = 0; - while ((inp_timestamps[inp_timestamps.size() - 1] >= - std::chrono::duration_cast>(t - t0).count()) and - executing_traj_) - { - while (inp_timestamps[j] <= std::chrono::duration_cast>(t - t0).count() && - j < inp_timestamps.size() - 1) - { - j += 1; - } - positions = UrDriver::interp_cubic(std::chrono::duration_cast>(t - t0).count() - - inp_timestamps[j - 1], - inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], - inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions); - - // oversample with 4 * sample_time - std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); - t = std::chrono::high_resolution_clock::now(); - } - executing_traj_ = false; - // Signal robot to stop driverProg() - UrDriver::closeServo(positions); - return true; -} - -void UrDriver::servoj(std::vector positions, int keepalive) -{ - if (!reverse_connected_) - { - print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " + - std::to_string(keepalive)); - return; - } - unsigned int bytes_written; - int tmp; - unsigned char buf[28]; - for (int i = 0; i < 6; i++) - { - tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_)); - buf[i * 4] = tmp & 0xff; - buf[i * 4 + 1] = (tmp >> 8) & 0xff; - buf[i * 4 + 2] = (tmp >> 16) & 0xff; - buf[i * 4 + 3] = (tmp >> 24) & 0xff; - } - tmp = htonl((int)keepalive); - buf[6 * 4] = tmp & 0xff; - buf[6 * 4 + 1] = (tmp >> 8) & 0xff; - buf[6 * 4 + 2] = (tmp >> 16) & 0xff; - buf[6 * 4 + 3] = (tmp >> 24) & 0xff; - bytes_written = write(new_sockfd_, buf, 28); -} - -void UrDriver::stopTraj() -{ - executing_traj_ = false; - rt_interface_->addCommandToQueue("stopj(10)\n"); -} - -bool UrDriver::uploadProg() -{ - std::string cmd_str; - char buf[128]; - cmd_str = "def driverProg():\n"; - - sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); - cmd_str += buf; - - cmd_str += "\tSERVO_IDLE = 0\n"; - cmd_str += "\tSERVO_RUNNING = 1\n"; - cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; - cmd_str += "\tdef set_servo_setpoint(q):\n"; - cmd_str += "\t\tenter_critical\n"; - cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; - cmd_str += "\t\tcmd_servo_q = q\n"; - cmd_str += "\t\texit_critical\n"; - cmd_str += "\tend\n"; - cmd_str += "\tthread servoThread():\n"; - cmd_str += "\t\tstate = SERVO_IDLE\n"; - cmd_str += "\t\twhile True:\n"; - cmd_str += "\t\t\tenter_critical\n"; - cmd_str += "\t\t\tq = cmd_servo_q\n"; - cmd_str += "\t\t\tdo_brake = False\n"; - cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; - cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; - cmd_str += "\t\t\t\tdo_brake = True\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\t\tstate = cmd_servo_state\n"; - cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n"; - cmd_str += "\t\t\texit_critical\n"; - cmd_str += "\t\t\tif do_brake:\n"; - cmd_str += "\t\t\t\tstopj(1.0)\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; - - if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", servoj_time_, servoj_lookahead_time_, - servoj_gain_); - else - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); - cmd_str += buf; - - cmd_str += "\t\t\telse:\n"; - cmd_str += "\t\t\t\tsync()\n"; - cmd_str += "\t\t\tend\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - - sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), REVERSE_PORT_); - cmd_str += buf; - - cmd_str += "\tthread_servo = run servoThread()\n"; - cmd_str += "\tkeepalive = 1\n"; - cmd_str += "\twhile keepalive > 0:\n"; - cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n"; - cmd_str += "\t\tif params_mult[0] > 0:\n"; - cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; - cmd_str += "params_mult[2] / MULT_jointstate, "; - cmd_str += "params_mult[3] / MULT_jointstate, "; - cmd_str += "params_mult[4] / MULT_jointstate, "; - cmd_str += "params_mult[5] / MULT_jointstate, "; - cmd_str += "params_mult[6] / MULT_jointstate]\n"; - cmd_str += "\t\t\tkeepalive = params_mult[7]\n"; - cmd_str += "\t\t\tset_servo_setpoint(q)\n"; - cmd_str += "\t\tend\n"; - cmd_str += "\tend\n"; - cmd_str += "\tsleep(.1)\n"; - cmd_str += "\tsocket_close()\n"; - cmd_str += "\tkill thread_servo\n"; - cmd_str += "end\n"; - - rt_interface_->addCommandToQueue(cmd_str); - return UrDriver::openServo(); -} - -bool UrDriver::openServo() -{ - struct sockaddr_in cli_addr; - socklen_t clilen; - clilen = sizeof(cli_addr); - new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, &clilen); - if (new_sockfd_ < 0) - { - print_fatal("ERROR on accepting reverse communication"); - return false; - } - reverse_connected_ = true; - return true; -} -void UrDriver::closeServo(std::vector positions) -{ - if (positions.size() != 6) - UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); - else - UrDriver::servoj(positions, 0); - - reverse_connected_ = false; - close(new_sockfd_); -} - -bool UrDriver::start() -{ - if (!sec_interface_->start()) - return false; - firmware_version_ = sec_interface_->robot_state_->getVersion(); - rt_interface_->robot_state_->setVersion(firmware_version_); - if (!rt_interface_->start()) - return false; - ip_addr_ = rt_interface_->getLocalIp(); - print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + "\n"); - return true; -} - -void UrDriver::halt() -{ - if (executing_traj_) - { - UrDriver::stopTraj(); - } - sec_interface_->halt(); - rt_interface_->halt(); - close(incoming_sockfd_); -} - -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); -} - -std::vector UrDriver::getJointNames() -{ - return joint_names_; -} - -void UrDriver::setJointNames(std::vector jn) -{ - joint_names_ = jn; -} - -void UrDriver::setToolVoltage(unsigned int v) -{ - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} -void UrDriver::setFlag(unsigned int n, bool b) -{ - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False"); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} -void UrDriver::setDigitalOut(unsigned int n, bool b) -{ - char buf[256]; - if (firmware_version_ < 2) - { - sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); - } - else if (n > 15) - { - sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", n - 16, b ? "True" : "False"); - } - else if (n > 7) - { - sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", n - 8, b ? "True" : "False"); - } - else - { - sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); - } - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} -void UrDriver::setAnalogOut(unsigned int n, double f) -{ - char buf[256]; - if (firmware_version_ < 2) - { - sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); - } - else - { - sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f); - } - - rt_interface_->addCommandToQueue(buf); - print_debug(buf); -} - -bool UrDriver::setPayload(double m) -{ - if ((m < maximum_payload_) && (m > minimum_payload_)) - { - char buf[256]; - sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); - rt_interface_->addCommandToQueue(buf); - print_debug(buf); - return true; - } - else - return false; -} - -void UrDriver::setMinPayload(double m) -{ - if (m > 0) - { - minimum_payload_ = m; - } - else - { - minimum_payload_ = 0; - } -} -void UrDriver::setMaxPayload(double m) -{ - maximum_payload_ = m; -} -void UrDriver::setServojTime(double t) -{ - if (t > 0.008) - { - servoj_time_ = t; - } - else - { - servoj_time_ = 0.008; - } -} -void UrDriver::setServojLookahead(double t) -{ - if (t > 0.03) - { - if (t < 0.2) - { - servoj_lookahead_time_ = t; - } - else - { - servoj_lookahead_time_ = 0.2; - } - } - else - { - servoj_lookahead_time_ = 0.03; - } -} -void UrDriver::setServojGain(double g) -{ - if (g > 100) - { - if (g < 2000) - { - servoj_gain_ = g; - } - else - { - servoj_gain_ = 2000; - } - } - else - { - servoj_gain_ = 100; - } -} diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp deleted file mode 100644 index 06b59f9..0000000 --- a/src/ur_hardware_interface.cpp +++ /dev/null @@ -1,283 +0,0 @@ -/* - * ur_hardware_control_loop.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* Based on original source from University of Colorado, Boulder. License copied below. */ - -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - ********************************************************************* - - Author: Dave Coleman - */ - -#include - -namespace ros_control_ur -{ -UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : nh_(nh), robot_(robot) -{ - // Initialize shared memory and interfaces here - init(); // this implementation loads from rosparam - - max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - - ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); -} - -void UrHardwareInterface::init() -{ - ROS_INFO_STREAM_NAMED("ur_hardware_interface", "Reading rosparams from namespace: " << nh_.getNamespace()); - - // Get joint names - nh_.getParam("hardware_interface/joints", joint_names_); - if (joint_names_.size() == 0) - { - ROS_FATAL_STREAM_NAMED("ur_hardware_interface", - "No joints found on parameter server for controller, did you load the proper yaml file?" - << " Namespace: " << nh_.getNamespace()); - exit(-1); - } - num_joints_ = joint_names_.size(); - - // Resize vectors - joint_position_.resize(num_joints_); - joint_velocity_.resize(num_joints_); - joint_effort_.resize(num_joints_); - joint_position_command_.resize(num_joints_); - joint_velocity_command_.resize(num_joints_); - prev_joint_velocity_command_.resize(num_joints_); - - // Initialize controller - for (std::size_t i = 0; i < num_joints_; ++i) - { - ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", "Loading joint name: " << joint_names_[i]); - - // Create joint state interface - joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_position_[i], - &joint_velocity_[i], &joint_effort_[i])); - - // Create position joint interface - position_joint_interface_.registerHandle(hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); - - // Create velocity joint interface - velocity_joint_interface_.registerHandle(hardware_interface::JointHandle( - joint_state_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i])); - prev_joint_velocity_command_[i] = 0.; - } - - // Create force torque interface - force_torque_interface_.registerHandle( - hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_)); - - registerInterface(&joint_state_interface_); // From RobotHW base class. - registerInterface(&position_joint_interface_); // From RobotHW base class. - registerInterface(&velocity_joint_interface_); // From RobotHW base class. - registerInterface(&force_torque_interface_); // From RobotHW base class. - velocity_interface_running_ = false; - position_interface_running_ = false; -} - -void UrHardwareInterface::read() -{ - std::vector pos, vel, current, tcp; - pos = robot_->rt_interface_->robot_state_->getQActual(); - vel = robot_->rt_interface_->robot_state_->getQdActual(); - current = robot_->rt_interface_->robot_state_->getIActual(); - tcp = robot_->rt_interface_->robot_state_->getTcpForce(); - for (std::size_t i = 0; i < num_joints_; ++i) - { - joint_position_[i] = pos[i]; - joint_velocity_[i] = vel[i]; - joint_effort_[i] = current[i]; - } - for (std::size_t i = 0; i < 3; ++i) - { - robot_force_[i] = tcp[i]; - robot_torque_[i] = tcp[i + 3]; - } -} - -void UrHardwareInterface::setMaxVelChange(double inp) -{ - max_vel_change_ = inp; -} - -void UrHardwareInterface::write() -{ - if (velocity_interface_running_) - { - std::vector cmd; - // do some rate limiting - cmd.resize(joint_velocity_command_.size()); - for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) - { - cmd[i] = joint_velocity_command_[i]; - if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) - { - cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_; - } - else if (cmd[i] < prev_joint_velocity_command_[i] - max_vel_change_) - { - cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_; - } - prev_joint_velocity_command_[i] = cmd[i]; - } - robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], max_vel_change_ * 125); - } - else if (position_interface_running_) - { - robot_->servoj(joint_position_command_); - } -} - -bool UrHardwareInterface::canSwitch(const std::list& start_list, - const std::list& stop_list) const -{ - for (std::list::const_iterator controller_it = start_list.begin(); - controller_it != start_list.end(); ++controller_it) - { - if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - if (velocity_interface_running_) - { - ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (position_interface_running_) - { - bool error = true; - for (std::list::const_iterator stop_controller_it = stop_list.begin(); - stop_controller_it != stop_list.end(); ++stop_controller_it) - { - if (stop_controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - error = false; - break; - } - } - if (error) - { - ROS_ERROR("%s (type %s) can not be run simultaneously with a PositionJointInterface", - controller_it->name.c_str(), controller_it->hardware_interface.c_str()); - return false; - } - } - } - else if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - if (position_interface_running_) - { - ROS_ERROR("%s: An interface of that type (%s) is already running", controller_it->name.c_str(), - controller_it->hardware_interface.c_str()); - return false; - } - if (velocity_interface_running_) - { - bool error = true; - for (std::list::const_iterator stop_controller_it = stop_list.begin(); - stop_controller_it != stop_list.end(); ++stop_controller_it) - { - if (stop_controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - error = false; - break; - } - } - if (error) - { - ROS_ERROR("%s (type %s) can not be run simultaneously with a VelocityJointInterface", - controller_it->name.c_str(), controller_it->hardware_interface.c_str()); - return false; - } - } - } - } - - // we can always stop a controller - return true; -} - -void UrHardwareInterface::doSwitch(const std::list& start_list, - const std::list& stop_list) -{ - for (std::list::const_iterator controller_it = stop_list.begin(); - controller_it != stop_list.end(); ++controller_it) - { - if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - velocity_interface_running_ = false; - ROS_DEBUG("Stopping velocity interface"); - } - if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - position_interface_running_ = false; - std::vector tmp; - robot_->closeServo(tmp); - ROS_DEBUG("Stopping position interface"); - } - } - for (std::list::const_iterator controller_it = start_list.begin(); - controller_it != start_list.end(); ++controller_it) - { - if (controller_it->hardware_interface == "hardware_interface::VelocityJointInterface") - { - velocity_interface_running_ = true; - ROS_DEBUG("Starting velocity interface"); - } - if (controller_it->hardware_interface == "hardware_interface::PositionJointInterface") - { - position_interface_running_ = true; - robot_->uploadProg(); - ROS_DEBUG("Starting position interface"); - } - } -} - -} // namespace diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp deleted file mode 100644 index e947aa9..0000000 --- a/src/ur_realtime_communication.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* - * ur_realtime_communication.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_modern_driver/ur_realtime_communication.h" - -UrRealtimeCommunication::UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) -{ - robot_state_ = new RobotStateRT(msg_cond); - bzero((char*)&serv_addr_, sizeof(serv_addr_)); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) - { - print_fatal("ERROR opening socket"); - } - server_ = gethostbyname(host.c_str()); - if (server_ == NULL) - { - print_fatal("ERROR, no such host"); - } - 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_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - connected_ = false; - keepalive_ = false; - safety_count_ = safety_count_max + 1; - safety_count_max_ = safety_count_max; -} - -bool UrRealtimeCommunication::start() -{ - fd_set writefds; - struct timeval timeout; - - keepalive_ = true; - print_debug("Realtime port: Connecting..."); - - connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - timeout.tv_sec = 10; - timeout.tv_usec = 0; - select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_fatal("Error connecting to RT port 30003"); - return false; - } - sockaddr_in name; - socklen_t namelen = sizeof(name); - int err = getsockname(sockfd_, (sockaddr*)&name, &namelen); - if (err < 0) - { - print_fatal("Could not get local IP"); - close(sockfd_); - return false; - } - char str[18]; - inet_ntop(AF_INET, &name.sin_addr, str, 18); - local_ip_ = str; - comThread_ = std::thread(&UrRealtimeCommunication::run, this); - return true; -} - -void UrRealtimeCommunication::halt() -{ - keepalive_ = false; - comThread_.join(); -} - -void UrRealtimeCommunication::addCommandToQueue(std::string inp) -{ - int bytes_written; - if (inp.back() != '\n') - { - inp.append("\n"); - } - if (connected_) - bytes_written = write(sockfd_, inp.c_str(), inp.length()); - else - print_error("Could not send command \"" + inp + "\". The robot is not connected! Command is discarded"); -} - -void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc) -{ - char cmd[1024]; - if (robot_state_->getVersion() >= 3.1) - { - sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f)\n", q0, q1, q2, q3, q4, q5, acc); - } - else - { - sprintf(cmd, "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 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]; - int bytes_read; - bzero(buf, 2048); - struct timeval timeout; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sockfd_, &readfds); - print_debug("Realtime port: Got connection"); - connected_ = true; - while (keepalive_) - { - while (connected_ && keepalive_) - { - timeout.tv_sec = 0; // do this each loop as selects modifies timeout - timeout.tv_usec = 500000; // timeout of 0.5 sec - select(sockfd_ + 1, &readfds, NULL, NULL, &timeout); - bytes_read = read(sockfd_, buf, 2048); - if (bytes_read > 0) - { - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) - { - setSpeed(0., 0., 0., 0., 0., 0.); - } - safety_count_ += 1; - } - else - { - connected_ = false; - close(sockfd_); - } - } - if (keepalive_) - { - // reconnect - print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); - sockfd_ = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd_ < 0) - { - print_fatal("ERROR opening socket"); - } - flag_ = 1; - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag_, sizeof(int)); - setsockopt(sockfd_, IPPROTO_TCP, TCP_QUICKACK, (char*)&flag_, sizeof(int)); - - setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char*)&flag_, sizeof(int)); - fcntl(sockfd_, F_SETFL, O_NONBLOCK); - while (keepalive_ && !connected_) - { - std::this_thread::sleep_for(std::chrono::seconds(10)); - fd_set writefds; - - connect(sockfd_, (struct sockaddr*)&serv_addr_, sizeof(serv_addr_)); - FD_ZERO(&writefds); - FD_SET(sockfd_, &writefds); - select(sockfd_ + 1, NULL, &writefds, NULL, NULL); - unsigned int flag_len; - getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); - if (flag_ < 0) - { - print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 " - "seconds..."); - } - else - { - connected_ = true; - print_info("Realtime port: Reconnected"); - } - } - } - } - setSpeed(0., 0., 0., 0., 0., 0.); - close(sockfd_); -} - -void UrRealtimeCommunication::setSafetyCountMax(uint inp) -{ - safety_count_max_ = inp; -} - -std::string UrRealtimeCommunication::getLocalIp() -{ - return local_ip_; -} diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp deleted file mode 100644 index 6f3f854..0000000 --- a/src/ur_ros_wrapper.cpp +++ /dev/null @@ -1,871 +0,0 @@ -/* - * ur_driver.cpp - * - * Copyright 2015 Thomas Timm Andersen - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "ur_modern_driver/do_output.h" -#include "ur_modern_driver/ur_driver.h" -#include "ur_modern_driver/ur_hardware_interface.h" - -#include -#include -#include -#include "actionlib/server/action_server.h" -#include "actionlib/server/server_goal_handle.h" -#include "control_msgs/FollowJointTrajectoryAction.h" -#include "geometry_msgs/PoseStamped.h" -#include "geometry_msgs/WrenchStamped.h" -#include "ros/ros.h" -#include "sensor_msgs/JointState.h" -#include "std_msgs/String.h" -#include "trajectory_msgs/JointTrajectoryPoint.h" -#include "ur_msgs/Analog.h" -#include "ur_msgs/Digital.h" -#include "ur_msgs/IOStates.h" -#include "ur_msgs/SetIO.h" -#include "ur_msgs/SetIORequest.h" -#include "ur_msgs/SetIOResponse.h" -#include "ur_msgs/SetPayload.h" -#include "ur_msgs/SetPayloadRequest.h" -#include "ur_msgs/SetPayloadResponse.h" - -/// TF -#include -#include - -class RosWrapper -{ -protected: - UrDriver robot_; - std::condition_variable rt_msg_cond_; - std::condition_variable msg_cond_; - ros::NodeHandle nh_; - actionlib::ActionServer as_; - actionlib::ServerGoalHandle goal_handle_; - bool has_goal_; - control_msgs::FollowJointTrajectoryFeedback feedback_; - control_msgs::FollowJointTrajectoryResult result_; - ros::Subscriber speed_sub_; - ros::Subscriber urscript_sub_; - ros::ServiceServer io_srv_; - ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; - double io_flag_delay_; - double max_velocity_; - std::vector joint_offsets_; - std::string base_frame_; - std::string tool_frame_; - bool use_ros_control_; - std::thread* ros_control_thread_; - boost::shared_ptr hardware_interface_; - boost::shared_ptr controller_manager_; - -public: - RosWrapper(std::string host, int reverse_port) - : as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1), - boost::bind(&RosWrapper::cancelCB, this, _1), false) - , robot_(rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300) - , io_flag_delay_(0.05) - , joint_offsets_(6, 0.0) - { - std::string joint_prefix = ""; - std::vector joint_names; - char buf[256]; - - if (ros::param::get("~prefix", joint_prefix)) - { - if (joint_prefix.length() > 0) - { - sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); - print_info(buf); - } - } - joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "shoulder_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"); - robot_.setJointNames(joint_names); - - use_ros_control_ = false; - ros::param::get("~use_ros_control", use_ros_control_); - - if (use_ros_control_) - { - hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_)); - controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_)); - double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2 - if (ros::param::get("~max_acceleration", max_vel_change)) - { - max_vel_change = max_vel_change / 125; - } - sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change * 125); - print_debug(buf); - hardware_interface_->setMaxVelChange(max_vel_change); - } - // Using a very high value in order to not limit execution of trajectories being sent from MoveIt! - max_velocity_ = 10.; - if (ros::param::get("~max_velocity", max_velocity_)) - { - sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_); - print_debug(buf); - } - - // Bounds for SetPayload service - // Using a very conservative value as it should be set through the parameter server - double min_payload = 0.; - double max_payload = 1.; - if (ros::param::get("~min_payload", min_payload)) - { - sprintf(buf, "Min payload set to: %f [kg]", min_payload); - print_debug(buf); - } - if (ros::param::get("~max_payload", max_payload)) - { - sprintf(buf, "Max payload set to: %f [kg]", max_payload); - print_debug(buf); - } - robot_.setMinPayload(min_payload); - robot_.setMaxPayload(max_payload); - sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload); - print_debug(buf); - - double servoj_time = 0.008; - if (ros::param::get("~servoj_time", servoj_time)) - { - sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); - print_debug(buf); - } - robot_.setServojTime(servoj_time); - - double servoj_lookahead_time = 0.03; - if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) - { - sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); - print_debug(buf); - } - robot_.setServojLookahead(servoj_lookahead_time); - - double servoj_gain = 300.; - if (ros::param::get("~servoj_gain", servoj_gain)) - { - sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); - print_debug(buf); - } - robot_.setServojGain(servoj_gain); - - // Base and tool frames - base_frame_ = joint_prefix + "base_link"; - tool_frame_ = joint_prefix + "tool0_controller"; - if (ros::param::get("~base_frame", base_frame_)) - { - sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); - print_debug(buf); - } - if (ros::param::get("~tool_frame", tool_frame_)) - { - sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str()); - print_debug(buf); - } - - if (robot_.start()) - { - if (use_ros_control_) - { - ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this)); - print_debug("The control thread for this driver has been started"); - } - else - { - // start actionserver - has_goal_ = false; - as_.start(); - - // subscribe to the data topic of interest - rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this)); - print_debug("The action server for this driver has been started"); - } - mb_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishMbMsg, this)); - speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this); - urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this); - - io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this); - payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this); - } - } - - void halt() - { - robot_.halt(); - rt_publish_thread_->join(); - } - -private: - void trajThread(std::vector timestamps, std::vector> positions, - std::vector> velocities) - { - robot_.doTraj(timestamps, positions, velocities); - if (has_goal_) - { - result_.error_code = result_.SUCCESSFUL; - goal_handle_.setSucceeded(result_); - has_goal_ = false; - } - } - void goalCB(actionlib::ServerGoalHandle gh) - { - std::string buf; - print_info("on_goal"); - if (!robot_.sec_interface_->robot_state_->isReady()) - { - result_.error_code = -100; // nothing is defined for this...? - - if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) - { - result_.error_string = "Cannot accept new trajectories: Robot arm is not powered on"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) - { - result_.error_string = "Cannot accept new trajectories: Robot is not enabled"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - result_.error_string = "Cannot accept new trajectories. (Debug: Robot mode is " + - std::to_string(robot_.sec_interface_->robot_state_->getRobotMode()) + ")"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) - { - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Cannot accept new trajectories: Robot is emergency stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) - { - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Cannot accept new trajectories: Robot is protective stopped"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - actionlib::ActionServer::Goal goal = - *gh.getGoal(); // make a copy that we can modify - if (has_goal_) - { - print_warning("Received new goal while still executing previous trajectory. Canceling previous trajectory"); - has_goal_ = false; - robot_.stopTraj(); - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Received another trajectory"; - goal_handle_.setAborted(result_, result_.error_string); - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } - goal_handle_ = gh; - if (!validateJointNames()) - { - std::string outp_joint_names = ""; - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) - { - outp_joint_names += goal.trajectory.joint_names[i] + " "; - } - result_.error_code = result_.INVALID_JOINTS; - result_.error_string = "Received a goal with incorrect joint names: " + outp_joint_names; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - if (!has_positions()) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without positions"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_velocities()) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal without velocities"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!traj_is_finite()) - { - result_.error_string = "Received a goal with infinities or NaNs"; - result_.error_code = result_.INVALID_GOAL; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - if (!has_limited_velocities()) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_); - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - reorder_traj_joints(goal.trajectory); - - if (!start_positions_match(goal.trajectory, 0.01)) - { - result_.error_code = result_.INVALID_GOAL; - result_.error_string = "Goal start doesn't match current pose"; - gh.setRejected(result_, result_.error_string); - print_error(result_.error_string); - return; - } - - std::vector timestamps; - std::vector> positions, velocities; - if (goal.trajectory.points[0].time_from_start.toSec() != 0.) - { - print_warning("Trajectory's first point should be the current position, with time_from_start set to 0.0 - " - "Inserting point in malformed trajectory"); - timestamps.push_back(0.0); - positions.push_back(robot_.rt_interface_->robot_state_->getQActual()); - velocities.push_back(robot_.rt_interface_->robot_state_->getQdActual()); - } - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - timestamps.push_back(goal.trajectory.points[i].time_from_start.toSec()); - positions.push_back(goal.trajectory.points[i].positions); - velocities.push_back(goal.trajectory.points[i].velocities); - } - - goal_handle_.setAccepted(); - has_goal_ = true; - std::thread(&RosWrapper::trajThread, this, timestamps, positions, velocities).detach(); - } - - void cancelCB(actionlib::ServerGoalHandle gh) - { - // set the action state to preempted - print_info("on_cancel"); - if (has_goal_) - { - if (gh == goal_handle_) - { - robot_.stopTraj(); - has_goal_ = false; - } - } - result_.error_code = -100; // nothing is defined for this...? - result_.error_string = "Goal cancelled by client"; - gh.setCanceled(result_); - } - - bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) - { - resp.success = true; - // if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { - if (req.fun == 1) - { - robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); - } - else if (req.fun == 2) - { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { - robot_.setFlag(req.pin, req.state > 0.0 ? true : false); - // According to urdriver.py, set_flag will fail if called to rapidly in succession - ros::Duration(io_flag_delay_).sleep(); - } - else if (req.fun == 3) - { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { - robot_.setAnalogOut(req.pin, req.state); - } - else if (req.fun == 4) - { - //} else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { - robot_.setToolVoltage((int)req.state); - } - else - { - resp.success = false; - } - return resp.success; - } - - bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) - { - if (robot_.setPayload(req.payload)) - resp.success = true; - else - resp.success = true; - return resp.success; - } - - bool validateJointNames() - { - std::vector actual_joint_names = robot_.getJointNames(); - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - if (goal.trajectory.joint_names.size() != actual_joint_names.size()) - return false; - - for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) - { - unsigned int j; - for (j = 0; j < actual_joint_names.size(); j++) - { - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - break; - } - if (goal.trajectory.joint_names[i] == actual_joint_names[j]) - { - actual_joint_names.erase(actual_joint_names.begin() + j); - } - else - { - return false; - } - } - - return true; - } - - void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) - { - /* Reorders trajectory - destructive */ - std::vector actual_joint_names = robot_.getJointNames(); - std::vector mapping; - mapping.resize(actual_joint_names.size(), actual_joint_names.size()); - for (unsigned int i = 0; i < traj.joint_names.size(); i++) - { - for (unsigned int j = 0; j < actual_joint_names.size(); j++) - { - if (traj.joint_names[i] == actual_joint_names[j]) - mapping[j] = i; - } - } - traj.joint_names = actual_joint_names; - std::vector new_traj; - for (unsigned int i = 0; i < traj.points.size(); i++) - { - trajectory_msgs::JointTrajectoryPoint new_point; - for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) - { - new_point.positions.push_back(traj.points[i].positions[mapping[j]]); - new_point.velocities.push_back(traj.points[i].velocities[mapping[j]]); - if (traj.points[i].accelerations.size() != 0) - new_point.accelerations.push_back(traj.points[i].accelerations[mapping[j]]); - } - new_point.time_from_start = traj.points[i].time_from_start; - new_traj.push_back(new_point); - } - traj.points = new_traj; - } - - bool has_velocities() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - if (goal.trajectory.points[i].positions.size() != goal.trajectory.points[i].velocities.size()) - return false; - } - return true; - } - - bool has_positions() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - if (goal.trajectory.points.size() == 0) - return false; - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - if (goal.trajectory.points[i].positions.size() != goal.trajectory.joint_names.size()) - return false; - } - return true; - } - - bool start_positions_match(const trajectory_msgs::JointTrajectory& traj, double eps) - { - for (unsigned int i = 0; i < traj.points[0].positions.size(); i++) - { - std::vector qActual = robot_.rt_interface_->robot_state_->getQActual(); - if (fabs(traj.points[0].positions[i] - qActual[i]) > eps) - { - return false; - } - } - return true; - } - - bool has_limited_velocities() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) - { - if (fabs(goal.trajectory.points[i].velocities[j]) > max_velocity_) - return false; - } - } - return true; - } - - bool traj_is_finite() - { - actionlib::ActionServer::Goal goal = *goal_handle_.getGoal(); - for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) - { - for (unsigned int j = 0; j < goal.trajectory.points[i].velocities.size(); j++) - { - if (!std::isfinite(goal.trajectory.points[i].positions[j])) - return false; - if (!std::isfinite(goal.trajectory.points[i].velocities[j])) - return false; - } - } - return true; - } - - void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) - { - if (msg->points[0].velocities.size() == 6) - { - double acc = 100; - if (msg->points[0].accelerations.size() > 0) - acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end()); - robot_.setSpeed(msg->points[0].velocities[0], msg->points[0].velocities[1], msg->points[0].velocities[2], - msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc); - } - } - void urscriptInterface(const std_msgs::String::ConstPtr& msg) - { - robot_.rt_interface_->addCommandToQueue(msg->data); - } - - void rosControlLoop() - { - ros::Duration elapsed_time; - struct timespec last_time, current_time; - static const double BILLION = 1000000000.0; - - realtime_tools::RealtimePublisher tf_pub(nh_, "/tf", 1); - geometry_msgs::TransformStamped tool_transform; - tool_transform.header.frame_id = base_frame_; - tool_transform.child_frame_id = tool_frame_; - tf_pub.msg_.transforms.push_back(tool_transform); - - realtime_tools::RealtimePublisher tool_vel_pub(nh_, "tool_velocity", 1); - tool_vel_pub.msg_.header.frame_id = base_frame_; - - clock_gettime(CLOCK_MONOTONIC, &last_time); - while (ros::ok()) - { - 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 (!robot_.rt_interface_->robot_state_->getControllerUpdated()) - { - rt_msg_cond_.wait(locker); - } - // Input - hardware_interface_->read(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); - - // Control - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - elapsed_time = - ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec) / BILLION); - ros::Time ros_time = ros::Time::now(); - controller_manager_->update(ros_time, elapsed_time); - last_time = current_time; - - // Output - hardware_interface_->write(); - - // Tool vector: 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 tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - - // Compute rotation angle - double rx = tool_vector_actual[3]; - double ry = tool_vector_actual[4]; - double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); - - // Broadcast transform - if (tf_pub.trylock()) - { - tf_pub.msg_.transforms[0].header.stamp = ros_time; - if (angle < 1e-16) - { - tf_pub.msg_.transforms[0].transform.rotation.x = 0; - tf_pub.msg_.transforms[0].transform.rotation.y = 0; - tf_pub.msg_.transforms[0].transform.rotation.z = 0; - tf_pub.msg_.transforms[0].transform.rotation.w = 1; - } - else - { - tf_pub.msg_.transforms[0].transform.rotation.x = (rx / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.y = (ry / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.z = (rz / angle) * std::sin(angle * 0.5); - tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle * 0.5); - } - tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0]; - tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1]; - tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2]; - - tf_pub.unlockAndPublish(); - } - - // Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - - if (tool_vel_pub.trylock()) - { - tool_vel_pub.msg_.header.stamp = ros_time; - tool_vel_pub.msg_.twist.linear.x = tcp_speed[0]; - tool_vel_pub.msg_.twist.linear.y = tcp_speed[1]; - tool_vel_pub.msg_.twist.linear.z = tcp_speed[2]; - tool_vel_pub.msg_.twist.angular.x = tcp_speed[3]; - tool_vel_pub.msg_.twist.angular.y = tcp_speed[4]; - tool_vel_pub.msg_.twist.angular.z = tcp_speed[5]; - - tool_vel_pub.unlockAndPublish(); - } - } - } - - void publishRTMsg() - { - ros::Publisher joint_pub = nh_.advertise("joint_states", 1); - ros::Publisher wrench_pub = nh_.advertise("wrench", 1); - ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); - static tf::TransformBroadcaster br; - while (ros::ok()) - { - sensor_msgs::JointState joint_msg; - joint_msg.name = robot_.getJointNames(); - geometry_msgs::WrenchStamped wrench_msg; - geometry_msgs::PoseStamped tool_pose_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 (!robot_.rt_interface_->robot_state_->getDataPublished()) - { - rt_msg_cond_.wait(locker); - } - joint_msg.header.stamp = ros::Time::now(); - joint_msg.position = robot_.rt_interface_->robot_state_->getQActual(); - for (unsigned int i = 0; i < joint_msg.position.size(); i++) - { - joint_msg.position[i] += joint_offsets_[i]; - } - 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 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); - - // Tool vector: 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 tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - - // Create quaternion - tf::Quaternion quat; - double rx = tool_vector_actual[3]; - double ry = tool_vector_actual[4]; - double rz = tool_vector_actual[5]; - double angle = std::sqrt(std::pow(rx, 2) + std::pow(ry, 2) + std::pow(rz, 2)); - if (angle < 1e-16) - { - quat.setValue(0, 0, 0, 1); - } - else - { - quat.setRotation(tf::Vector3(rx / angle, ry / angle, rz / angle), angle); - } - - // Create and broadcast transform - tf::Transform transform; - transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2])); - transform.setRotation(quat); - br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); - - // Publish tool velocity - std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); - geometry_msgs::TwistStamped tool_twist; - tool_twist.header.frame_id = base_frame_; - tool_twist.header.stamp = joint_msg.header.stamp; - tool_twist.twist.linear.x = tcp_speed[0]; - tool_twist.twist.linear.y = tcp_speed[1]; - tool_twist.twist.linear.z = tcp_speed[2]; - tool_twist.twist.angular.x = tcp_speed[3]; - tool_twist.twist.angular.y = tcp_speed[4]; - tool_twist.twist.angular.z = tcp_speed[5]; - tool_vel_pub.publish(tool_twist); - - robot_.rt_interface_->robot_state_->setDataPublished(); - } - } - - void publishMbMsg() - { - bool warned = false; - ros::Publisher io_pub = nh_.advertise("ur_driver/io_states", 1); - - while (ros::ok()) - { - ur_msgs::IOStates io_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 (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) - { - msg_cond_.wait(locker); - } - int i_max = 10; - if (robot_.sec_interface_->robot_state_->getVersion() > 3.0) - i_max = 18; // From version 3.0, there are up to 18 inputs and outputs - for (unsigned int i = 0; i < i_max; i++) - { - ur_msgs::Digital digi; - digi.pin = i; - digi.state = ((robot_.sec_interface_->robot_state_->getDigitalInputBits() & (1 << i)) >> i); - io_msg.digital_in_states.push_back(digi); - digi.state = ((robot_.sec_interface_->robot_state_->getDigitalOutputBits() & (1 << i)) >> i); - io_msg.digital_out_states.push_back(digi); - } - ur_msgs::Analog ana; - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0(); - io_msg.analog_in_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1(); - io_msg.analog_in_states.push_back(ana); - - ana.pin = 0; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0(); - io_msg.analog_out_states.push_back(ana); - ana.pin = 1; - ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1(); - io_msg.analog_out_states.push_back(ana); - io_pub.publish(io_msg); - - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() or - robot_.sec_interface_->robot_state_->isProtectiveStopped()) - { - if (robot_.sec_interface_->robot_state_->isEmergencyStopped() and !warned) - { - print_error("Emergency stop pressed!"); - } - else if (robot_.sec_interface_->robot_state_->isProtectiveStopped() and !warned) - { - print_error("Robot is protective stopped!"); - } - if (has_goal_) - { - print_error("Aborting trajectory"); - robot_.stopTraj(); - result_.error_code = result_.SUCCESSFUL; - result_.error_string = "Robot was halted"; - goal_handle_.setAborted(result_, result_.error_string); - has_goal_ = false; - } - warned = true; - } - else - warned = false; - - robot_.sec_interface_->robot_state_->finishedReading(); - } - } -}; - -int main(int argc, char** argv) -{ - bool use_sim_time = false; - std::string host; - int reverse_port = 50001; - - ros::init(argc, argv, "ur_driver"); - ros::NodeHandle nh; - if (ros::param::get("use_sim_time", use_sim_time)) - { - print_warning("use_sim_time is set!!"); - } - if (!(ros::param::get("~robot_ip_address", host))) - { - if (argc > 1) - { - print_warning("Please set the parameter robot_ip_address instead of giving it as a command line argument. This " - "method is DEPRECATED"); - host = argv[1]; - } - else - { - print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as " - "robot_ip"); - exit(1); - } - } - if ((ros::param::get("~reverse_port", reverse_port))) - { - if ((reverse_port <= 0) or (reverse_port >= 65535)) - { - print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001"); - reverse_port = 50001; - } - } - else - reverse_port = 50001; - - RosWrapper interface(host, reverse_port); - - ros::AsyncSpinner spinner(3); - spinner.start(); - - ros::waitForShutdown(); - - interface.halt(); - - exit(0); -}