ROS试炼——UR5机器人配置、通讯、RVIZ-moveit控制、C++调用moveit控制

*注：配置：Ubuntu16.04+ROS kinetic

---

1.创建工作空间

$mkdir -p catkin_ws/src
进入到catkin_ws目录下，执行如下命令：
$catkin_make
*这个命令用于构建该工作空间，在catkin_ws路径下使用catkin_make命令
$source devel/setup.bash
*该命令是在catkin_ws目录下执行的，意思是把catkin_ws/devel目录下的setup.bash文件挂载到ROS的文件系统里去，这样当用户执行一些文件系统的命令时，就不会提示找不到

---

2.创建功能包

*功能包是一个存在于工作空间catkin_ws目录src目录下的一个目录，这个目录中包含一些文件或者目录，一个功能包必须以下几部分组成：
(1) 必须包括一个package.xml文件;
(2) 必须包括一个CMakeLists.txt文件。
*首先进入到目录catkin_ws/src目录下，创建一个名字为demo的功能包，它直接依赖于三个功能包：std_msgs,rospy以及roscpp，使用如下命令：
$catkin_create_pkg demo std_msgs rospy roscpp
使用rospack命令来查看功能包之间的依赖关系（使用rospack命令的前提是已经安装了该命令） 查看直接依赖关系：
$rospack depends1 demo
功能包创建好后，在工作空间catkin_ws路径下用catkin_make编译功能包,如果不出错，则功能包创建成功。
$catkin_make
该部分内容参考链接：创建工作空间及功能包

*********以上内容为创建工作空间前期准备，UR机器人的安装与编译，在此工作空间下调试

---

3.安装UR5相关的各种包

注：需要提前安装moveit,否则功能包编译不成功；执行如下命令

$sudo apt-get update

$sudo apt-get install ros-indigo-moveit-full

第二行代码如果执行不成功，则执行下面的代码

$sudo apt-get install ros-indigo-moveit*

---

(1) cd ~/catkin_ws/src （将路径定位在工作空间的src路径下）
(2) git clone -b kinetic-devel https://github.com/ros-industrial/universal_robot.git
( 下载universal_robot功能包， 备注：如果你的ROS系统版本不是kinetic，请将上面代码的kinetic换成相应的版本)
(3) cd ~/catkin_ws （定位到catkin_ws路径下）
(4) rosdep install --from-paths src --ignore-src --rosdistro indigo
(？？？什么意思？？？备注：同样的，如果你的ROS系统版本不是indigo，请将上面代码的indigo 换成相应的版本)
(5) catkin_make
( 6) source devel/setup.bash

---

4.ur_modern_driver驱动安装

虽然第二步中universal_robot文件夹下已经包含了ur_driver但这个驱动只支持比较早的UR5机械臂控制器的版本，换句话说，ur_modern_driver更适合（如果你的UR5本体的控制箱的软件版本是3.0以上）
具体的步骤：
（1）删除catkin_ws/src/universal_robot这个目录下的ur_driver文件夹，
（2）然后下载ur_modern_driver （可以直接下载，然后复制到universal_robot文件夹下，或者使用命令下载，如3.2步中方式）
（3）再解压，粘贴到原来的ur_driver这个文件夹的位置(把文件名更改为ur_modern_driver)。
（4）接着cd ~/catkin_ws
（5）最后catkin_make
**************************************编译过程中，出现错误，采用如下方法纠正
打开文件ur_hardware_interface.cpp；将 if (controller_it->hardware_interface) 改为 if (controller_it->type);即hardware_interface变为type；controller_it->hardware_interface.c_str()变为type.c_str()； if (stop_controller_it->hardware_interface) 改为 if (stop_controller_it->type);具体数量大概是12个。

---

5.UR5本体实物通信测试

首先打开进入Ubuntu系统打开一个新的终端
(1) cd ~/catkin_ws
(2) source devel/setup.bash
(3) roslaunch ur_modern_driver ur5_bringup.launch limited:=true robot_ip:=
IP_OF_THE_ROBOT [reverse_port:=REVERSE_PORT]
(备注：如果机器人通过网线与PC端相连接，IP_OF_THE_ROBOT需要替换成UR5机械臂本体（示教器中机器人网络设置）中的静态地址这种情况下保证机器人与PC在同一个网段下，即机器人与PC的IP地址只有最后一个小数点后的数不同，PC端的IP设置为有线连接，通过设置中的网络进行IP设置。若果机器人通过路由器与PC端相连接，IP_OF_THE_ROBOT需要替换成UR5机械臂本体的DHCP)
(4) roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch limited:=true
（备注：执行(4)之前开新的终端，先执行(2)再执行(4)，不然可能会报错。）
(5).roslaunch ur5_moveit_config moveit_rviz.launch config:=true
（备注，开新的终端，先执行4中的(2)再执行(5)，不然可能会报错）
如果一切正常， 你就会看到RVIZ中的UR5机械臂和实物的状态是一致的，拖动UR5机械臂实物，在RVIZ里的机械臂也会跟着运动。此时，你可以在RVIZ中用鼠标拖动机械臂到达目标位置，在planing下点击plan,如果路径规划成功即可点击execute,你就会看到UR5实物也会跟着运动到目标点。界面中可以调节机械臂运行速度。
这部分内容参考链接UR机器人通讯与控制

---

6.C++调用moveit控制UR5机器人运动

1.首先，创建一个新的功能包，来放置我们的代码：

$catkin_create_pkg puncture_demo catkin cmake_modules interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp std_msgs

2.在创建的功能包下创建puncture_demo.cpp文件；代码如下：

//punture_demo.cpp
#include 
#include 
 
#include  
#include  
#include  
#include  

int main(int argc, char **argv)
{
  ros::init(argc, argv, "movo_moveit");
  ros::NodeHandle node_handle; 
  ros::AsyncSpinner spinner(1);
  spinner.start();
 
  moveit::planning_interface::MoveGroup group("manipulator");//ur5对应moveit中选择的规划部分
 
  // 设置发送的数据，对应于moveit中的拖拽
  geometry_msgs::Pose target_pose1;
  
  target_pose1.orientation.x= -0.1993;
  target_pose1.orientation.y = 0.3054;
  target_pose1.orientation.z = -0.2284;
  target_pose1.orientation.w = 0.902;
 
  target_pose1.position.x = -0.2004;
  target_pose1.position.y = -1.0177;
  target_pose1.position.z = 1.56930;
 
  group.setPoseTarget(target_pose1);
  group.setMaxVelocityScalingFactor(0.02);
 
 
  // 进行运动规划，计算机器人移动到目标的运动轨迹，对应moveit中的plan按钮
  moveit::planning_interface::MoveGroup::Plan my_plan;
//bool success = (ptr_group->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
  bool success = group.plan(my_plan)== moveit::planning_interface::MoveItErrorCode::SUCCESS;;
 
  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   
 
  //让机械臂按照规划的轨迹开始运动，对应moveit中的execute。
  if(success)
      group.execute(my_plan);
 
///////////////////////////////////第二个位置////////////////////////////////////
geometry_msgs::Pose target_pose2;

   target_pose2.orientation.x= -0.1993;
  target_pose2.orientation.y = 0.3054;
  target_pose2.orientation.z = -0.2284;
  target_pose2.orientation.w = 0.902;
 
  target_pose2.position.x = 0.096;
  target_pose2.position.y = -0.9618;
  target_pose2.position.z = 1.934;
 
  group.setPoseTarget(target_pose2);
  group.setMaxVelocityScalingFactor(0.02);
 
  // moveit::planning_interface::MoveGroup::Plan my_plan;
 
  // 进行运动规划，计算机器人移动到目标的运动轨迹，对应moveit中的plan按钮
  moveit::planning_interface::MoveGroup::Plan my_plan_1;
//bool success = (ptr_group->plan(my_plan_1) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
  bool success1 = group.plan(my_plan_1)== moveit::planning_interface::MoveItErrorCode::SUCCESS;;
 
  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   
 
  //让机械臂按照规划的轨迹开始运动，对应moveit中的execute。
  if(success1)
      group.execute(my_plan_1);
////////////////////////////////////////////////第三个位置////////////////////////////
geometry_msgs::Pose target_pose3;
  
   target_pose3.orientation.x= -0.1993;
  target_pose3.orientation.y = 0.3054;
  target_pose3.orientation.z = -0.2284;
  target_pose3.orientation.w = 0.902;
 
  target_pose3.position.x = 0.1219;
  target_pose3.position.y = -0.9801;
  target_pose3.position.z = 1.9163;
 
  group.setPoseTarget(target_pose3);
  group.setMaxVelocityScalingFactor(0.01);
 
 
  // 进行运动规划，计算机器人移动到目标的运动轨迹，对应moveit中的plan按钮
  moveit::planning_interface::MoveGroup::Plan my_plan_2;
//bool success = (ptr_group->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
  bool success2 = group.plan(my_plan_2)== moveit::planning_interface::MoveItErrorCode::SUCCESS;;
 
  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   
 
  //让机械臂按照规划的轨迹开始运动，对应moveit中的execute。
  if(success2)
      group.execute(my_plan_2);
  
  ros::shutdown(); 
  return 0;
}

注：程序控制机器人运动三个位置，分别为 target_pose1、 target_pose2、 target_pose3；三段程序相同，对应的my_plan、my_plan_1、my_plan_2；success也需要修改。
3.更改puncture_demo功能包下的CMakeLists.txt文件
将如下代码添加到下面：

add_executable(puncture_demo src/puncture_demo.cpp)
target_link_libraries(puncture_demo
  ${catkin_LIBRARIES} 
)

×××××××××××××××××××××××程序运行时出现错误××××××××××××××××××××××
参考如下进行调试：
错误及改正方法
×××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××
编译程序
在工作空间catkin_ws下编译功能包：catkin_make
编译成功会在catkin_ws/devel/lib/puncture_demo/文件夹下出现执行文件puncture_demo文件
执行如下命令即可控制机器人运动：
$rosrun puncture_demo puncture_demo

注：机器人执行过程中，打开新终端，执行第5步的过程

---

7.机器人环境配置

机器人运行过程中，需要重新配置环境：机器人没有安装在地面上，需更改机器人环境配置文件
找到catkin_ws/src/universal_robot-kinetic-devel/ur_description/urdf/ur5_joint_limited_robot.urdf.xacro文件
修改代码如下：

相对于源文件，添加了