mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
Included a position-based controller. Also prettied up printing
This commit is contained in:
Thomas Timm Andersen
@@ -156,7 +156,8 @@ set(${PROJECT_NAME}_SOURCES
|
||||
src/ur_realtime_communication.cpp
|
||||
src/ur_communication.cpp
|
||||
src/robot_state.cpp
|
||||
src/robot_state_RT.cpp)
|
||||
src/robot_state_RT.cpp
|
||||
src/do_output.cpp)
|
||||
add_executable(ur_driver ${${PROJECT_NAME}_SOURCES})
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
|
||||
@@ -22,7 +22,43 @@ force_torque_sensor_controller:
|
||||
type: force_torque_sensor_controller/ForceTorqueSensorController
|
||||
publish_rate: 125
|
||||
|
||||
# Joint Trajectory Controller -------------------------------
|
||||
# Joint Trajectory Controller - position based -------------------------------
|
||||
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
|
||||
position_based_position_trajectory_controller:
|
||||
type: position_controllers/JointTrajectoryController
|
||||
joints:
|
||||
- shoulder_pan_joint
|
||||
- shoulder_lift_joint
|
||||
- elbow_joint
|
||||
- wrist_1_joint
|
||||
- wrist_2_joint
|
||||
- wrist_3_joint
|
||||
constraints:
|
||||
goal_time: 0.6
|
||||
stopped_velocity_tolerance: 0.05
|
||||
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
|
||||
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
|
||||
elbow_joint: {trajectory: 0.1, goal: 0.1}
|
||||
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
|
||||
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
|
||||
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
|
||||
stop_trajectory_duration: 0.5
|
||||
state_publish_rate: 125
|
||||
action_monitor_rate: 10
|
||||
gains:
|
||||
#!!These values have not been optimized!!
|
||||
shoulder_pan_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
|
||||
shoulder_lift_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
|
||||
elbow_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
|
||||
wrist_1_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
|
||||
wrist_2_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
|
||||
wrist_3_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1}
|
||||
|
||||
# state_publish_rate: 50 # Defaults to 50
|
||||
# action_monitor_rate: 20 # Defaults to 20
|
||||
#hold_trajectory_duration: 0 # Defaults to 0.5
|
||||
|
||||
# Joint Trajectory Controller - velocity based -------------------------------
|
||||
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
|
||||
velocity_based_position_trajectory_controller:
|
||||
type: velocity_controllers/JointTrajectoryController
|
||||
|
||||
27
include/ur_modern_driver/do_output.h
Normal file
27
include/ur_modern_driver/do_output.h
Normal file
@@ -0,0 +1,27 @@
|
||||
/*
|
||||
* do_output.h
|
||||
*
|
||||
* ----------------------------------------------------------------------------
|
||||
* "THE BEER-WARE LICENSE" (Revision 42):
|
||||
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
|
||||
* can do whatever you want with this stuff. If we meet some day, and you think
|
||||
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
|
||||
* ----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef UR_DO_OUTPUT_H_
|
||||
#define UR_DO_OUTPUT_H_
|
||||
|
||||
#ifdef ROS_BUILD
|
||||
#include <ros/ros.h>
|
||||
#endif
|
||||
#include <string>
|
||||
|
||||
void print_debug(std::string inp);
|
||||
void print_info(std::string inp);
|
||||
void print_warning(std::string inp);
|
||||
void print_error(std::string inp);
|
||||
void print_fatal(std::string inp);
|
||||
|
||||
|
||||
#endif /* UR_DO_OUTPUT_H_ */
|
||||
@@ -13,6 +13,7 @@
|
||||
#define UR_COMMUNICATION_H_
|
||||
|
||||
#include "robot_state.h"
|
||||
#include "do_output.h"
|
||||
#include <vector>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
@@ -32,9 +33,6 @@
|
||||
#include <fcntl.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#ifdef ROS_BUILD
|
||||
#include <ros/ros.h>
|
||||
#endif
|
||||
|
||||
class UrCommunication {
|
||||
private:
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
#include <condition_variable>
|
||||
#include "ur_realtime_communication.h"
|
||||
#include "ur_communication.h"
|
||||
#include "do_output.h"
|
||||
#include <vector>
|
||||
#include <math.h>
|
||||
#include <string>
|
||||
@@ -65,7 +66,7 @@ public:
|
||||
|
||||
void uploadProg();
|
||||
void openServo();
|
||||
void closeServo();
|
||||
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,
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
* ----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
/* Based on original source from University of Colorado, Boulder. License copied below. Feel free to offer Dave a beer as well if you meet him. */
|
||||
/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */
|
||||
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
@@ -58,6 +58,7 @@
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <boost/scoped_ptr.hpp>
|
||||
#include <ros/ros.h>
|
||||
#include "do_output.h"
|
||||
#include "ur_driver.h"
|
||||
|
||||
namespace ros_control_ur {
|
||||
@@ -85,7 +86,11 @@ public:
|
||||
/// \brief write the command to the robot hardware.
|
||||
virtual void write();
|
||||
|
||||
void doSwitch(const std::list<hardware_interface::ControllerInfo>&, const std::list<hardware_interface::ControllerInfo>&);
|
||||
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:
|
||||
|
||||
@@ -95,17 +100,20 @@ protected:
|
||||
// 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::size_t num_joints_;
|
||||
double robot_force_[3] = {0.,0.,0.};
|
||||
double robot_torque_[3] = {0.,0.,0.};
|
||||
double robot_force_[3] = { 0., 0., 0. };
|
||||
double robot_torque_[3] = { 0., 0., 0. };
|
||||
|
||||
// Robot API
|
||||
UrDriver* robot_;
|
||||
|
||||
@@ -13,6 +13,7 @@
|
||||
#define UR_REALTIME_COMMUNICATION_H_
|
||||
|
||||
#include "robot_state_RT.h"
|
||||
#include "do_output.h"
|
||||
#include <vector>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
@@ -33,10 +34,6 @@
|
||||
#include <fcntl.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#ifdef ROS_BUILD
|
||||
#include <ros/ros.h>
|
||||
#endif
|
||||
|
||||
class UrRealtimeCommunication {
|
||||
private:
|
||||
unsigned int safety_count_max_;
|
||||
|
||||
@@ -30,8 +30,8 @@
|
||||
<rosparam file="$(find ur_modern_driver)/config/ur5_controllers.yaml" command="load"/>
|
||||
|
||||
<!-- Load controller manager -->
|
||||
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
|
||||
output="screen" args="spawn joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
|
||||
<node name="ros_control_controller_manager" pkg="controller_manager" type="spawner" respawn="false"
|
||||
output="screen" args="joint_state_controller force_torque_sensor_controller position_based_position_trajectory_controller" />
|
||||
|
||||
<!-- Convert joint states to /tf tranforms -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
|
||||
|
||||
50
src/do_output.cpp
Normal file
50
src/do_output.cpp
Normal file
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
* do_output.cpp
|
||||
*
|
||||
* ----------------------------------------------------------------------------
|
||||
* "THE BEER-WARE LICENSE" (Revision 42):
|
||||
* <thomas.timm.dk@gmail.com> wrote this file. As long as you retain this notice you
|
||||
* can do whatever you want with this stuff. If we meet some day, and you think
|
||||
* this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen
|
||||
* ----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "ur_modern_driver/du_output.h"
|
||||
|
||||
void print_debug(std::string inp) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG(inp);
|
||||
#else
|
||||
printf("DEBUG: %s\n", inp.c_str());
|
||||
#endif
|
||||
}
|
||||
void print_info(std::string inp) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_INFO(inp);
|
||||
#else
|
||||
printf("INFO: %s\n", inp.c_str());
|
||||
#endif
|
||||
}
|
||||
void print_warning(std::string inp) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_WARN(inp);
|
||||
#else
|
||||
printf("WARNING: %s\n", inp.c_str());
|
||||
#endif
|
||||
}
|
||||
void print_error(std::string inp) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_ERROR(inp);
|
||||
#else
|
||||
printf("ERROR: %s\n", inp.c_str());
|
||||
#endif
|
||||
}
|
||||
void print_fatal(std::string inp) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL(inp);
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("FATAL: %s\n", inp.c_str());
|
||||
exit(1);
|
||||
#endif
|
||||
}
|
||||
@@ -18,33 +18,15 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond,
|
||||
bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_));
|
||||
pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (pri_sockfd_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR opening socket");
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("ERROR opening socket");
|
||||
exit(1);
|
||||
#endif
|
||||
print_fatal("ERROR opening socket pri_sockfd");
|
||||
}
|
||||
sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sec_sockfd_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR opening socket");
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("ERROR opening socket");
|
||||
exit(1);
|
||||
#endif
|
||||
print_fatal("ERROR opening socket sec_sockfd");
|
||||
}
|
||||
server_ = gethostbyname(host.c_str());
|
||||
if (server_ == NULL) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR, no such host");
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("ERROR, no such host\n");
|
||||
exit(1);
|
||||
#endif
|
||||
print_fatal("ERROR, unknown host");
|
||||
}
|
||||
pri_serv_addr_.sin_family = AF_INET;
|
||||
sec_serv_addr_.sin_family = AF_INET;
|
||||
@@ -72,46 +54,26 @@ bool UrCommunication::start() {
|
||||
unsigned int bytes_read;
|
||||
std::string cmd;
|
||||
bzero(buf, 512);
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Acquire firmware version: Connecting...");
|
||||
#else
|
||||
printf("Acquire firmware version: Connecting...\n");
|
||||
#endif
|
||||
print_debug("Acquire firmware version: Connecting...");
|
||||
if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_,
|
||||
sizeof(pri_serv_addr_)) < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("Error connecting to get firmware version");
|
||||
ros::shutdown();
|
||||
print_fatal("Error connecting to get firmware version");
|
||||
return false;
|
||||
#else
|
||||
printf("Error connecting to get firmware version\n");
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Acquire firmware version: Got connection");
|
||||
#else
|
||||
printf("Acquire firmware version: Got connection\n");
|
||||
#endif
|
||||
print_debug("Acquire firmware version: Got connection");
|
||||
bytes_read = read(pri_sockfd_, buf, 512);
|
||||
setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (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));
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Firmware version detected: %1.7f", robot_state_->getVersion());
|
||||
#else
|
||||
printf("Firmware version detected: %f\n", robot_state_->getVersion());
|
||||
#endif
|
||||
char tmp[64];
|
||||
sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion());
|
||||
print_debug(tmp);
|
||||
close(pri_sockfd_);
|
||||
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Switching to secondary interface for masterboard data: Connecting...");
|
||||
#else
|
||||
printf(
|
||||
"Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic
|
||||
#endif
|
||||
print_debug(
|
||||
"Switching to secondary interface for masterboard data: Connecting...");
|
||||
|
||||
fd_set writefds;
|
||||
struct timeval timeout;
|
||||
@@ -126,20 +88,10 @@ bool UrCommunication::start() {
|
||||
unsigned int flag_len;
|
||||
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
|
||||
if (flag_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("Error connecting to secondary interface");
|
||||
ros::shutdown();
|
||||
print_fatal("Error connecting to secondary interface");
|
||||
return false;
|
||||
#else
|
||||
printf("Error connecting to secondary interface\n");
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Secondary interface: Got connection");
|
||||
#else
|
||||
printf("Secondary interface: Got connection\n");
|
||||
#endif
|
||||
print_debug("Secondary interface: Got connection");
|
||||
comThread_ = std::thread(&UrCommunication::run, this);
|
||||
return true;
|
||||
}
|
||||
@@ -170,26 +122,15 @@ void UrCommunication::run() {
|
||||
robot_state_->unpack(buf, bytes_read);
|
||||
} else {
|
||||
connected_ = false;
|
||||
close (sec_sockfd_);
|
||||
close(sec_sockfd_);
|
||||
}
|
||||
}
|
||||
if (keepalive_) {
|
||||
//reconnect
|
||||
#ifdef ROS_BUILD
|
||||
ROS_WARN("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
|
||||
#else
|
||||
printf(
|
||||
"Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n");
|
||||
#endif
|
||||
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) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR opening secondary socket");
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("ERROR opening secondary socket");
|
||||
exit(1);
|
||||
#endif
|
||||
print_fatal("ERROR opening secondary socket");
|
||||
}
|
||||
flag_ = 1;
|
||||
setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
|
||||
@@ -211,12 +152,7 @@ void UrCommunication::run() {
|
||||
getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_,
|
||||
&flag_len);
|
||||
if (flag_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_ERROR("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
|
||||
#else
|
||||
printf(
|
||||
"Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
|
||||
#endif
|
||||
print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds...");
|
||||
} else {
|
||||
connected_ = true;
|
||||
}
|
||||
|
||||
@@ -24,15 +24,12 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
|
||||
|
||||
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) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR opening socket for reverse communication");
|
||||
#else
|
||||
printf("ERROR opening socket for reverse communication");
|
||||
#endif
|
||||
print_fatal("ERROR opening socket for reverse communication");
|
||||
}
|
||||
bzero((char *) &serv_addr, sizeof(serv_addr));
|
||||
|
||||
@@ -45,11 +42,7 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
|
||||
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
|
||||
if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr,
|
||||
sizeof(serv_addr)) < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR on binding socket for reverse communication");
|
||||
#else
|
||||
printf("ERROR on binding socket for reverse communication");
|
||||
#endif
|
||||
print_fatal("ERROR on binding socket for reverse communication");
|
||||
}
|
||||
listen(incoming_sockfd_, 5);
|
||||
}
|
||||
@@ -158,9 +151,7 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
|
||||
t = std::chrono::high_resolution_clock::now();
|
||||
}
|
||||
//Signal robot to stop driverProg()
|
||||
UrDriver::servoj(positions, 0.008, 0);
|
||||
UrDriver::closeServo();
|
||||
|
||||
UrDriver::closeServo(positions);
|
||||
}
|
||||
|
||||
void UrDriver::servoj(std::vector<double> positions, double time,
|
||||
@@ -277,14 +268,14 @@ void UrDriver::openServo() {
|
||||
new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr,
|
||||
&clilen);
|
||||
if (new_sockfd_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR on accepting reverse communication");
|
||||
#else
|
||||
printf("ERROR on accepting reverse communication");
|
||||
#endif
|
||||
print_fatal("ERROR on accepting reverse communication");
|
||||
}
|
||||
}
|
||||
void UrDriver::closeServo() {
|
||||
void UrDriver::closeServo(std::vector<double> positions) {
|
||||
if (positions.size() != 6)
|
||||
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0.008, 0);
|
||||
else
|
||||
UrDriver::servoj(positions, 0.008, 0);
|
||||
close(new_sockfd_);
|
||||
}
|
||||
|
||||
@@ -296,11 +287,9 @@ bool UrDriver::start() {
|
||||
if (!rt_interface_->start())
|
||||
return false;
|
||||
ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr);
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
|
||||
#else
|
||||
printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
|
||||
#endif
|
||||
char buf[256];
|
||||
sprintf(buf, "Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_);
|
||||
print_debug(buf);
|
||||
return true;
|
||||
|
||||
}
|
||||
@@ -327,62 +316,37 @@ void UrDriver::setJointNames(std::vector<std::string> jn) {
|
||||
void UrDriver::setToolVoltage(unsigned int v) {
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("%s", buf);
|
||||
#else
|
||||
printf("%s", buf);
|
||||
#endif
|
||||
|
||||
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");
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("%s", buf);
|
||||
#else
|
||||
printf("%s", buf);
|
||||
#endif
|
||||
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
|
||||
print_debug(buf);
|
||||
}
|
||||
void UrDriver::setDigitalOut(unsigned int n, bool b) {
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
|
||||
b ? "True" : "False");
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("%s", buf);
|
||||
#else
|
||||
printf("%s", buf);
|
||||
#endif
|
||||
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
|
||||
}
|
||||
void UrDriver::setAnalogOut(unsigned int n, double f) {
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("%s", buf);
|
||||
#else
|
||||
printf("%s", buf);
|
||||
#endif
|
||||
|
||||
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);
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("%s", buf);
|
||||
#else
|
||||
printf("%s", buf);
|
||||
#endif
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
return true;
|
||||
} else
|
||||
return false;
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
* ----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
/* Based on original source from University of Colorado, Boulder. License copied below. Feel free to offer Dave a beer as well if you meet him. */
|
||||
/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */
|
||||
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
@@ -77,6 +77,7 @@ void UrHardwareInterface::init() {
|
||||
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_);
|
||||
|
||||
// Initialize controller
|
||||
@@ -90,6 +91,12 @@ void UrHardwareInterface::init() {
|
||||
&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(
|
||||
@@ -97,15 +104,17 @@ void UrHardwareInterface::init() {
|
||||
&joint_velocity_command_[i]));
|
||||
}
|
||||
|
||||
// Create joint state interface
|
||||
// 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() {
|
||||
@@ -131,9 +140,81 @@ void UrHardwareInterface::write() {
|
||||
robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1],
|
||||
joint_velocity_command_[2], joint_velocity_command_[3],
|
||||
joint_velocity_command_[4], joint_velocity_command_[5], 100);
|
||||
} else if (position_interface_running_) {
|
||||
robot_->servoj(joint_position_command_, 0.008, 1);
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
@@ -145,6 +226,12 @@ void UrHardwareInterface::doSwitch(
|
||||
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");
|
||||
}
|
||||
}
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
|
||||
stop_list.begin(); controller_it != stop_list.end();
|
||||
@@ -154,22 +241,14 @@ void UrHardwareInterface::doSwitch(
|
||||
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");
|
||||
}
|
||||
/*std::string outp;
|
||||
outp = "doSwitch called - Start list:";
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
|
||||
start_list.begin(); controller_it != start_list.end();
|
||||
++controller_it) {
|
||||
outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
|
||||
}
|
||||
outp += " - Stop list:";
|
||||
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
|
||||
stop_list.begin(); controller_it != stop_list.end();
|
||||
++controller_it) {
|
||||
outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
|
||||
|
||||
}
|
||||
ROS_INFO(outp.c_str()); */
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -18,23 +18,11 @@ UrRealtimeCommunication::UrRealtimeCommunication(
|
||||
bzero((char *) &serv_addr_, sizeof(serv_addr_));
|
||||
sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sockfd_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR opening socket");
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("ERROR opening socket");
|
||||
exit(1);
|
||||
#endif
|
||||
print_fatal("ERROR opening socket");
|
||||
}
|
||||
server_ = gethostbyname(host.c_str());
|
||||
if (server_ == NULL) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR, no such host");
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("ERROR, no such host\n");
|
||||
exit(1);
|
||||
#endif
|
||||
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);
|
||||
@@ -54,11 +42,7 @@ bool UrRealtimeCommunication::start() {
|
||||
struct timeval timeout;
|
||||
|
||||
keepalive_ = true;
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Realtime port: Connecting...");
|
||||
#else
|
||||
printf("Realtime port: Connecting...\n");
|
||||
#endif
|
||||
print_debug("Realtime port: Connecting...");
|
||||
|
||||
connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_));
|
||||
FD_ZERO(&writefds);
|
||||
@@ -69,25 +53,14 @@ bool UrRealtimeCommunication::start() {
|
||||
unsigned int flag_len;
|
||||
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
|
||||
if (flag_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("Error connecting to RT port 30003 - errno: %d (%s)", flag_, strerror(flag_));
|
||||
|
||||
#else
|
||||
printf("Error connecting to RT port 30003 - errno: %d (%s)\n", flag_,
|
||||
strerror(flag_));
|
||||
#endif
|
||||
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) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("Could not get local IP - errno: %d (%s)", errno, strerror(errno));
|
||||
#else
|
||||
printf("Could not get local IP - errno: %d (%s)", errno,
|
||||
strerror(errno));
|
||||
#endif
|
||||
print_fatal("Could not get local IP");
|
||||
close(sockfd_);
|
||||
return false;
|
||||
}
|
||||
@@ -132,11 +105,7 @@ void UrRealtimeCommunication::run() {
|
||||
fd_set readfds;
|
||||
FD_ZERO(&readfds);
|
||||
FD_SET(sockfd_, &readfds);
|
||||
#ifdef ROS_BUILD
|
||||
ROS_DEBUG("Realtime port: Got connection");
|
||||
#else
|
||||
printf("Realtime port: Got connection\n");
|
||||
#endif
|
||||
print_debug("Realtime port: Got connection");
|
||||
connected_ = true;
|
||||
while (keepalive_) {
|
||||
while (connected_ && keepalive_) {
|
||||
@@ -159,21 +128,10 @@ void UrRealtimeCommunication::run() {
|
||||
}
|
||||
if (keepalive_) {
|
||||
//reconnect
|
||||
#ifdef ROS_BUILD
|
||||
ROS_WARN("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...");
|
||||
#else
|
||||
printf(
|
||||
"Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n");
|
||||
#endif
|
||||
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) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_FATAL("ERROR opening socket");
|
||||
ros::shutdown();
|
||||
#else
|
||||
printf("ERROR opening socket");
|
||||
exit(1);
|
||||
#endif
|
||||
print_fatal("ERROR opening socket");
|
||||
}
|
||||
flag_ = 1;
|
||||
setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
|
||||
@@ -194,12 +152,7 @@ void UrRealtimeCommunication::run() {
|
||||
unsigned int flag_len;
|
||||
getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len);
|
||||
if (flag_ < 0) {
|
||||
#ifdef ROS_BUILD
|
||||
ROS_ERROR("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
|
||||
#else
|
||||
printf(
|
||||
"Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
|
||||
#endif
|
||||
print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds...");
|
||||
} else {
|
||||
connected_ = true;
|
||||
}
|
||||
|
||||
@@ -11,6 +11,7 @@
|
||||
|
||||
#include "ur_modern_driver/ur_driver.h"
|
||||
#include "ur_modern_driver/ur_hardware_interface.h"
|
||||
#include "ur_modern_driver/do_output.h"
|
||||
#include <string.h>
|
||||
#include <vector>
|
||||
#include <mutex>
|
||||
@@ -71,9 +72,11 @@ public:
|
||||
|
||||
std::string joint_prefix = "";
|
||||
std::vector<std::string> joint_names;
|
||||
char buf[256];
|
||||
|
||||
if (ros::param::get("~prefix", joint_prefix)) {
|
||||
ROS_INFO("Setting prefix to %s", joint_prefix.c_str());
|
||||
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");
|
||||
@@ -96,8 +99,9 @@ public:
|
||||
//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_)) {
|
||||
ROS_DEBUG("Max velocity accepted by ur_driver: %f [rad/s]",
|
||||
sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]",
|
||||
max_velocity_);
|
||||
print_debug(buf);
|
||||
}
|
||||
|
||||
//Bounds for SetPayload service
|
||||
@@ -105,21 +109,24 @@ public:
|
||||
double min_payload = 0.;
|
||||
double max_payload = 1.;
|
||||
if (ros::param::get("~min_payload", min_payload)) {
|
||||
ROS_DEBUG("Min payload set to: %f [kg]", min_payload);
|
||||
sprintf(buf, "Min payload set to: %f [kg]", min_payload);
|
||||
print_debug(buf);
|
||||
}
|
||||
if (ros::param::get("~max_payload", max_payload)) {
|
||||
ROS_DEBUG("Max payload set to: %f [kg]", max_payload);
|
||||
sprintf(buf, "Max payload set to: %f [kg]", max_payload);
|
||||
print_debug(buf);
|
||||
}
|
||||
robot_.setMinPayload(min_payload);
|
||||
robot_.setMaxPayload(max_payload);
|
||||
ROS_DEBUG("Bounds for set_payload service calls: [%f, %f]", min_payload,
|
||||
max_payload);
|
||||
sprintf(buf, "Bounds for set_payload service calls: [%f, %f]",
|
||||
min_payload, max_payload);
|
||||
print_debug(buf);
|
||||
|
||||
if (robot_.start()) {
|
||||
if (use_ros_control_) {
|
||||
ros_control_thread_ = new std::thread(
|
||||
boost::bind(&RosWrapper::rosControlLoop, this));
|
||||
ROS_DEBUG(
|
||||
print_debug(
|
||||
"The control thread for this driver has been started");
|
||||
} else {
|
||||
//register the goal and feedback callbacks
|
||||
@@ -133,7 +140,7 @@ public:
|
||||
//subscribe to the data topic of interest
|
||||
rt_publish_thread_ = new std::thread(
|
||||
boost::bind(&RosWrapper::publishRTMsg, this));
|
||||
ROS_DEBUG("The action server for this driver has been started");
|
||||
print_debug("The action server for this driver has been started");
|
||||
}
|
||||
mb_publish_thread_ = new std::thread(
|
||||
boost::bind(&RosWrapper::publishMbMsg, this));
|
||||
@@ -156,7 +163,7 @@ public:
|
||||
}
|
||||
private:
|
||||
void goalCB() {
|
||||
ROS_INFO("on_goal");
|
||||
print_info("on_goal");
|
||||
|
||||
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>::GoalConstPtr goal =
|
||||
as_.acceptNewGoal();
|
||||
@@ -172,21 +179,21 @@ private:
|
||||
"Received a goal with incorrect joint names: "
|
||||
+ outp_joint_names;
|
||||
as_.setAborted(result_, result_.error_string);
|
||||
ROS_ERROR("%s", result_.error_string.c_str());
|
||||
print_error(result_.error_string);
|
||||
}
|
||||
|
||||
if (!has_velocities()) {
|
||||
result_.error_code = result_.INVALID_GOAL;
|
||||
result_.error_string = "Received a goal without velocities";
|
||||
as_.setAborted(result_, result_.error_string);
|
||||
ROS_ERROR("%s", result_.error_string.c_str());
|
||||
print_error(result_.error_string);
|
||||
}
|
||||
|
||||
if (!traj_is_finite()) {
|
||||
result_.error_string = "Received a goal with infinites or NaNs";
|
||||
result_.error_code = result_.INVALID_GOAL;
|
||||
as_.setAborted(result_, result_.error_string);
|
||||
ROS_ERROR("%s", result_.error_string.c_str());
|
||||
print_error(result_.error_string);
|
||||
}
|
||||
|
||||
if (!has_limited_velocities()) {
|
||||
@@ -194,7 +201,7 @@ private:
|
||||
result_.error_string =
|
||||
"Received a goal with velocities that are higher than %f", max_velocity_;
|
||||
as_.setAborted(result_, result_.error_string);
|
||||
ROS_ERROR("%s", result_.error_string.c_str());
|
||||
print_error(result_.error_string);
|
||||
}
|
||||
|
||||
reorder_traj_joints(goal_.trajectory);
|
||||
@@ -213,7 +220,7 @@ private:
|
||||
}
|
||||
|
||||
void preemptCB() {
|
||||
ROS_INFO("on_cancel");
|
||||
print_info("on_cancel");
|
||||
// set the action state to preempted
|
||||
robot_.stopTraj();
|
||||
as_.setPreempted();
|
||||
@@ -481,15 +488,15 @@ int main(int argc, char **argv) {
|
||||
ros::init(argc, argv, "ur_driver");
|
||||
ros::NodeHandle nh;
|
||||
if (ros::param::get("use_sim_time", use_sim_time)) {
|
||||
ROS_WARN("use_sim_time is set!!");
|
||||
print_warning("use_sim_time is set!!");
|
||||
}
|
||||
if (!(ros::param::get("~robot_ip_address", host))) {
|
||||
if (argc > 1) {
|
||||
ROS_WARN(
|
||||
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 {
|
||||
ROS_FATAL(
|
||||
print_fatal(
|
||||
"Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user