ROS（基于kinetic）与UR3之间通信

由于ROSwiki并未提及有关于UR3与ROS如何通信，针对ur3的v3.7.0版本控制器如何更改ur_driver驱动包，经过一番实验发现ROS-wiki上面UR5，UR10教程也适用UR3，但是直接把ur_modern_driver替换ur_driver，catkin_make后会报错，该文章就此把整个过程详细描述一遍
该教程使用注意：
1 .ur_modern_driver似乎有多个版本，按本文ur_modern_driver链接应该没问题
2 .使用的是台式机，ubuntu16.04，kinetic
3.本人菜鸟一枚，如有错误，劳请斧正，谢谢

1.准备工作

1.1 安装universal-robot

sudo apt-get install ros-kinetic-universal-robot

1.2下载universal_robot到功能包下src文件夹里

cd /path/to/catkin_ws/src
# retrieve the sources (replace '$DISTRO' with the ROS version you are using)

git clone -b $DISTRO-devel https://github.com/ros-industrial/universal_robot.git
cd /path/to/catkin_ws

# checking dependencies (replace '$DISTRO' with the ROS version you are using)
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro $DISTRO
#注意上面的替换

# building
catkin_make
# source this workspace (careful when also sourcing others)

source /path/to/catkin_ws/devel/setup.bash#注意自己终端是否是bash
#如果catkin_ws是用过的工作空间，省略上面这一步

上面指令的路径需要跟据自己的工作空间调整，总之，要把git clone的文件放在工作空间的src下，让universal_robot以功能包的身份存在于工作空间下。

1.3 ur_modern_driver的更改

由于我们实验室的UR3控制器是3.7.0，而上一步下载的universal_robot/ur_driver适用于v1.5.7849 --v1.8.23117版本，我们需要下载新的驱动ur_modern_driver，但是ros wiki给的驱动链接下载下来后，替换ur_driver后经编译会出错，解决如下：

1.3.1 删除universal_robot/ur_driver，并执行如下操作

➜  ~ cd ~/My_First_catkin_ws/src/universal_robot 
#我的工作空间位于home/My_First_catkin_ws，自己相应调整
➜  universal_robot git:(kinetic-devel) ✗ git clone https://github.com/iron-ox/ur_modern_driver.git

把github中的文件下载到universal_robot

1.3.2 替换My_First_catkin_ws/src/universal_robcatkin_makeot/ur_modern_driver/src/ur_hardware_interface.cpp的内容

替换内容如下

#include 

namespace ros_control_ur {

UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) :
		nh_(nh), robot_(robot) {
	// Initialize shared memory and interfaces here
	init(); // this implementation loads from rosparam

	max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2

	ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
}

void UrHardwareInterface::init() {
	ROS_INFO_STREAM_NAMED("ur_hardware_interface",
			"Reading rosparams from namespace: " << nh_.getNamespace());

	// Get joint names
	nh_.getParam("hardware_interface/joints", joint_names_);
	if (joint_names_.size() == 0) {
		ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
				"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
		exit(-1);
	}
	num_joints_ = joint_names_.size();

	// Resize vectors
	joint_position_.resize(num_joints_);
	joint_velocity_.resize(num_joints_);
	joint_effort_.resize(num_joints_);
	joint_position_command_.resize(num_joints_);
	joint_velocity_command_.resize(num_joints_);
	prev_joint_velocity_command_.resize(num_joints_);

	// Initialize controller
	for (std::size_t i = 0; i < num_joints_; ++i) {
		ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
				"Loading joint name: " << joint_names_[i]);

		// Create joint state interface
		joint_state_interface_.registerHandle(
				hardware_interface::JointStateHandle(joint_names_[i],
						&joint_position_[i], &joint_velocity_[i],
						&joint_effort_[i]));

		// Create position joint interface
		position_joint_interface_.registerHandle(
				hardware_interface::JointHandle(
						joint_state_interface_.getHandle(joint_names_[i]),
						&joint_position_command_[i]));

		// Create velocity joint interface
		velocity_joint_interface_.registerHandle(
				hardware_interface::JointHandle(
						joint_state_interface_.getHandle(joint_names_[i]),
						&joint_velocity_command_[i]));
		prev_joint_velocity_command_[i] = 0.;
	}

	// Create force torque interface
	force_torque_interface_.registerHandle(
			hardware_interface::ForceTorqueSensorHandle("wrench", "",
					robot_force_, robot_torque_));

	registerInterface(&joint_state_interface_); // From RobotHW base class.
	registerInterface(&position_joint_interface_); // From RobotHW base class.
	registerInterface(&velocity_joint_interface_); // From RobotHW base class.
	registerInterface(&force_torque_interface_); // From RobotHW base class.
	velocity_interface_running_ = false;
	position_interface_running_ = false;
}

void UrHardwareInterface::read() {
	std::vector<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->type
				== "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->type.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->type
							== "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->type.c_str());
					return false;
				}
			}
		} else if (controller_it->type
				== "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->type.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->type
							== "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->type.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->type
				== "hardware_interface::VelocityJointInterface") {
			velocity_interface_running_ = false;
			ROS_DEBUG("Stopping velocity interface");
		}
		if (controller_it->type
				== "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->type
				== "hardware_interface::VelocityJointInterface") {
			velocity_interface_running_ = true;
			ROS_DEBUG("Starting velocity interface");
		}
		if (controller_it->type
				== "hardware_interface::PositionJointInterface") {
			position_interface_running_ = true;
			robot_->uploadProg();
			ROS_DEBUG("Starting position interface");
		}
	}

}

} // namespace

1.3.3 替换后编译

➜  ~ cd My_First_catkin_ws 
➜  My_First_catkin_ws catkin_make

这时因该不会出错了

2.基于kinetic的ROS与连接UR3

2.1 将UR3连接宽带（网线从控制箱底部插入）

示教器如图设置使用ping指令ping一下控制箱的IP address，检查电脑同控制箱网络是否处于可连接状态

➜  ~ ping 192.168.1.102

2.2 启动launch，连接UR3

roslaunch ur_modern_driver ur3_bringup.launch robot_ip:=192.168.1.102[reverse_port:=REVERSE_PORT]

2.3.1 运行test_move.py 节点，测试机械臂是否动作

➜  ~ rosrun ur_modern_driver test_move.py 

我第一次运行该节点时，机械臂并未动作（这次是第二次测试，有点绝望），Ctrl+c终止后;再次运行test_move.py节点后，UR3机械臂开始动作；
2.3.2 也可以通过MoveIt控制机械臂动作
首先运行

roslaunch ur3_moveit_config ur3_moveit_planning_execution.launch

再运行

roslaunch ur3_moveit_config moveit_rviz.launch config:=true

在rviz插件中拖动终端腕关节坐标系到另外一个位置，点击规划，rviz中的UR3机械臂会规划一条可行路径；点击执行，rviz中的UR3机械臂与真实机械臂会按照规划的路径执行；

最后附上MoveIt功能包架构图，move_group是MoveIt的核心节点；上述2.3.1与2.3.2实际分别是下图中使用**moveit_commander（Python接口）与GUI（rviz插件接口）**控制机械臂

引用资料：
1.古月居博客
2.ROS-Industrial Tutorials