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

Clean up of old driver files

Browse Source
This commit is contained in:
Simon Rasmussen
2017-07-09 02:45:02 +02:00
parent 40fc986e7b
commit fdaaacfe2c
17 changed files with 5 additions and 2863 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
CMakeLists.txt
View File

@@ -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 ") 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() 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 ## Specify additional locations of header files
## Your package locations should be listed before other locations ## Your package locations should be listed before other locations
@@ -146,7 +146,9 @@ include_directories(include
## Declare a C++ library ## Declare a C++ library
# Hardware Interface # 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 target_link_libraries(ur_hardware_interface
${catkin_LIBRARIES} ${catkin_LIBRARIES}
) )
@@ -159,8 +161,6 @@ target_link_libraries(ur_hardware_interface
## Declare a C++ executable ## Declare a C++ executable
set(${PROJECT_NAME}_SOURCES set(${PROJECT_NAME}_SOURCES
src/ros/action_server.cpp src/ros/action_server.cpp
src/ros/controller.cpp
src/ros/hardware_interface.cpp
src/ros/mb_publisher.cpp src/ros/mb_publisher.cpp
src/ros/rt_publisher.cpp src/ros/rt_publisher.cpp
src/ros/service_stopper.cpp src/ros/service_stopper.cpp
@@ -172,13 +172,7 @@ set(${PROJECT_NAME}_SOURCES
src/ur/master_board.cpp src/ur/master_board.cpp
src/ur/rt_state.cpp src/ur/rt_state.cpp
src/ur/messages.cpp src/ur/messages.cpp
src/tcp_socket.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)
add_executable(ur_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp) add_executable(ur_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp)

8
include/ur_modern_driver/packet.h
View File

@@ -1,8 +0,0 @@
#pragma once
#include "ur_modern_driver/bin_parser.h"
class Packet
{
public:
virtual bool parseWith(BinParser& bp) = 0;
};

9
include/ur_modern_driver/parser.h
View File

@@ -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<Packet> parse(BinParser& bp) = 0;
};

233
include/ur_modern_driver/robot_state.h
View File

@@ -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 <inttypes.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#include <condition_variable>
#include <mutex>
#include <vector>
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<double> getQTarget();
int getDigitalInputBits();
int getDigitalOutputBits();
char getAnalogInputRange0();
char getAnalogInputRange1();
double getAnalogInput0();
double getAnalogInput1();
char getAnalogOutputDomain0();
char getAnalogOutputDomain1();
double getAnalogOutput0();
double getAnalogOutput1();
std::vector<double> 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_ */

119
include/ur_modern_driver/robot_state_RT.h
View File

@@ -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 <inttypes.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#include <condition_variable>
#include <mutex>
#include <vector>
class RobotStateRT
{
private:
double version_; // protocol version
double time_; // Time elapsed since the controller was started
std::vector<double> q_target_; // Target joint positions
std::vector<double> qd_target_; // Target joint velocities
std::vector<double> qdd_target_; // Target joint accelerations
std::vector<double> i_target_; // Target joint currents
std::vector<double> m_target_; // Target joint moments (torques)
std::vector<double> q_actual_; // Actual joint positions
std::vector<double> qd_actual_; // Actual joint velocities
std::vector<double> i_actual_; // Actual joint currents
std::vector<double> i_control_; // Joint control currents
std::vector<double> tool_vector_actual_; // Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
// and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_actual_; // Actual speed of the tool given in Cartesian coordinates
std::vector<double> tcp_force_; // Generalised forces in the TC
std::vector<double> tool_vector_target_; // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
// and rz is a rotation vector representation of the tool orientation
std::vector<double> tcp_speed_target_; // Target speed of the tool given in Cartesian coordinates
std::vector<bool> digital_input_bits_; // Current state of the digital inputs. NOTE: these are bits encoded as
// int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
std::vector<double> motor_temperatures_; // Temperature of each joint in degrees celsius
double controller_timer_; // Controller realtime thread execution time
double robot_mode_; // Robot mode
std::vector<double> joint_modes_; // Joint control modes
double safety_mode_; // Safety mode
std::vector<double> tool_accelerometer_values_; // Tool x,y and z accelerometer values (software version 1.7)
double speed_scaling_; // Speed scaling of the trajectory limiter
double linear_momentum_norm_; // Norm of Cartesian linear momentum
double v_main_; // Masterboard: Main voltage
double v_robot_; // Matorborad: Robot voltage (48V)
double i_robot_; // Masterboard: Robot current
std::vector<double> v_actual_; // Actual joint voltages
std::mutex val_lock_; // Locks the variables while unpack parses data;
std::condition_variable* pMsg_cond_; // Signals that new vars are available
bool data_published_; // to avoid spurious wakes
bool controller_updated_; // to avoid spurious wakes
std::vector<double> unpackVector(uint8_t* buf, int start_index, int nr_of_vals);
std::vector<bool> unpackDigitalInputBits(int64_t data);
double ntohd(uint64_t nf);
public:
RobotStateRT(std::condition_variable& msg_cond);
~RobotStateRT();
double getVersion();
double getTime();
std::vector<double> getQTarget();
std::vector<double> getQdTarget();
std::vector<double> getQddTarget();
std::vector<double> getITarget();
std::vector<double> getMTarget();
std::vector<double> getQActual();
std::vector<double> getQdActual();
std::vector<double> getIActual();
std::vector<double> getIControl();
std::vector<double> getToolVectorActual();
std::vector<double> getTcpSpeedActual();
std::vector<double> getTcpForce();
std::vector<double> getToolVectorTarget();
std::vector<double> getTcpSpeedTarget();
std::vector<bool> getDigitalInputBits();
std::vector<double> getMotorTemperatures();
double getControllerTimer();
double getRobotMode();
std::vector<double> getJointModes();
double getSafety_mode();
std::vector<double> getToolAccelerometerValues();
double getSpeedScaling();
double getLinearMomentumNorm();
double getVMain();
double getVRobot();
double getIRobot();
void setVersion(double ver);
void setDataPublished();
bool getDataPublished();
bool getControllerUpdated();
void setControllerUpdated();
std::vector<double> getVActual();
void unpack(uint8_t* buf);
};
#endif /* ROBOT_STATE_RT_H_ */

53
include/ur_modern_driver/ros/robot_hardware.h
View File

@@ -1,53 +0,0 @@
#pragma once
#include <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#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<std::string, HardwareInterface*> available_interfaces_;
template <typename T>
void registerHardwareInterface(T* interface)
{
registerInterface<typename T::parent_type>(interface);
available_interfaces_[hardware_interface::internal::demangledTypeName<typename T::parent_type>()] = interface;
}
public:
RobotHardware(URCommander& commander, std::vector<std::string>& 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<hardware_interface::JointStateInterface>(&joint_interface_);
registerInterface<hardware_interface::ForceTorqueSensorInterface>(&wrench_interface_);
registerHardwareInterface(&position_interface_);
registerHardwareInterface(&velocity_interface_);
}
// bool canSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list) const;
void doSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& 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();
};

63
include/ur_modern_driver/ur_communication.h
View File

@@ -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 <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <unistd.h>
#include <chrono>
#include <condition_variable>
#include <iostream>
#include <mutex>
#include <thread>
#include <vector>
#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_ */

97
include/ur_modern_driver/ur_driver.h
View File

@@ -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 <math.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <condition_variable>
#include <mutex>
#include <string>
#include <vector>
#include "do_output.h"
#include "ur_communication.h"
#include "ur_realtime_communication.h"
#include <chrono>
class UrDriver
{
private:
double maximum_time_step_;
double minimum_payload_;
double maximum_payload_;
std::vector<std::string> 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<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
std::vector<std::vector<double>> inp_velocities);
void servoj(std::vector<double> positions, int keepalive = 1);
void stopTraj();
bool uploadProg();
bool openServo();
void closeServo(std::vector<double> positions);
std::vector<double> interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel);
std::vector<std::string> getJointNames();
void setJointNames(std::vector<std::string> 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_ */

136
include/ur_modern_driver/ur_hardware_interface.h
View File

@@ -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 <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <math.h>
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#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<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) const;
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& 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<std::string> joint_names_;
std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
std::vector<double> joint_effort_;
std::vector<double> joint_position_command_;
std::vector<double> joint_velocity_command_;
std::vector<double> 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

73
include/ur_modern_driver/ur_realtime_communication.h
View File

@@ -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 <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/types.h>
#include <unistd.h>
#include <condition_variable>
#include <iostream>
#include <mutex>
#include <thread>
#include <vector>
#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_ */

62
src/do_output.cpp
View File

@@ -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
}

30
src/ros/robot_hardware.cpp
View File

@@ -1,30 +0,0 @@
#include "ur_modern_driver/ros/robot_hardware.h"
/*
bool RobotHardware::canSwitch(const std::list<ControllerInfo>& start_list,
const std::list<ControllerInfo>& 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;
}
*/

184
src/ur_communication.cpp
View File

@@ -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_);
}

420
src/ur_driver.cpp
View File

@@ -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<double> UrDriver::interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
std::vector<double> p0_vel, std::vector<double> p1_vel)
{
/*Returns positions of the joints at time 't' */
std::vector<double> 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<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
std::vector<std::vector<double>> inp_velocities)
{
std::chrono::high_resolution_clock::time_point t0, t;
std::vector<double> 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<std::chrono::duration<double>>(t - t0).count()) and
executing_traj_)
{
while (inp_timestamps[j] <= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() &&
j < inp_timestamps.size() - 1)
{
j += 1;
}
positions = UrDriver::interp_cubic(std::chrono::duration_cast<std::chrono::duration<double>>(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<double> 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<double> 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<std::string> UrDriver::getJointNames()
{
return joint_names_;
}
void UrDriver::setJointNames(std::vector<std::string> 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;
}
}

283
src/ur_hardware_interface.cpp
View File

@@ -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 <ur_modern_driver/ur_hardware_interface.h>
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<double> 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<double> 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<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) const
{
for (std::list<hardware_interface::ControllerInfo>::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<hardware_interface::ControllerInfo>::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<hardware_interface::ControllerInfo>::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<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
for (std::list<hardware_interface::ControllerInfo>::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<double> tmp;
robot_->closeServo(tmp);
ROS_DEBUG("Stopping position interface");
}
}
for (std::list<hardware_interface::ControllerInfo>::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

211
src/ur_realtime_communication.cpp
View File

@@ -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_;
}

871
src/ur_ros_wrapper.cpp
View File

@@ -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 <string.h>
#include <time.h>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <mutex>
#include <thread>
#include <vector>
#include "ur_modern_driver/do_output.h"
#include "ur_modern_driver/ur_driver.h"
#include "ur_modern_driver/ur_hardware_interface.h"
#include <controller_manager/controller_manager.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/console.h>
#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 <tf/tf.h>
#include <tf/transform_broadcaster.h>
class RosWrapper
{
protected:
UrDriver robot_;
std::condition_variable rt_msg_cond_;
std::condition_variable msg_cond_;
ros::NodeHandle nh_;
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> 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<double> joint_offsets_;
std::string base_frame_;
std::string tool_frame_;
bool use_ros_control_;
std::thread* ros_control_thread_;
boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
boost::shared_ptr<controller_manager::ControllerManager> 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<std::string> 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<double> timestamps, std::vector<std::vector<double>> positions,
std::vector<std::vector<double>> 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<control_msgs::FollowJointTrajectoryAction> 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<control_msgs::FollowJointTrajectoryAction>::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<double> timestamps;
std::vector<std::vector<double>> 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<control_msgs::FollowJointTrajectoryAction> 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<std::string> actual_joint_names = robot_.getJointNames();
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::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<std::string> actual_joint_names = robot_.getJointNames();
std::vector<unsigned int> 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<trajectory_msgs::JointTrajectoryPoint> 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<control_msgs::FollowJointTrajectoryAction>::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<control_msgs::FollowJointTrajectoryAction>::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<double> 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<control_msgs::FollowJointTrajectoryAction>::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<control_msgs::FollowJointTrajectoryAction>::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::tfMessage> 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<geometry_msgs::TwistStamped> 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<std::mutex> 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, &current_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<double> 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<double> 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<sensor_msgs::JointState>("joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("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<std::mutex> 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<double> tcp_force = robot_.rt_interface_->robot_state_->getTcpForce();
wrench_msg.header.stamp = joint_msg.header.stamp;
wrench_msg.wrench.force.x = tcp_force[0];
wrench_msg.wrench.force.y = tcp_force[1];
wrench_msg.wrench.force.z = tcp_force[2];
wrench_msg.wrench.torque.x = tcp_force[3];
wrench_msg.wrench.torque.y = tcp_force[4];
wrench_msg.wrench.torque.z = tcp_force[5];
wrench_pub.publish(wrench_msg);
// 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<double> 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<double> 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_msgs::IOStates>("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<std::mutex> 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);
}