【从零开始的ROS四轴机械臂控制】（五）- 构建运动控制服务

【从零开始的ROS四轴机械臂控制（五）】

八、运动控制节点
- 1.定义服务GoToPosition.srv
- 2.修改CMakeLists.txt
- 3.修改package.xml
- 4.构建包
- 5.arm_mover节点代码
- 6.Arm Mover的启动和互动
- - （1）修改gazebo.launch
  - （2）测试arm_mover服务

八、运动控制节点

运动控制服务,它负责指挥机械臂移动,命令遵arm循预定轨迹，arm_mover 节点提供服务arm_mover ，其允许系统中的其他节点发送 movement_commands 。

除了允许通过服务接口进行移动之外， arm_mover 还允许通过使用参数来配置最小和最大关节角度。

此部分完成效果如下，可以通过命令定义每个joint的角度。

1.定义服务GoToPosition.srv

首先为arm定义一个服务,命名为GoToPosition.srv：

$ cd ~/catkin_ws/src/simple_arm/
$ mkdir srv
$ cd srv
$ touch GoToPosition.srv

编辑GoToPosition.srv文件：

float64 joint1
float64 joint2
float64 joint3
float64 joint4
float64 right_joint
float64 left_joint
---
duration time_elapsed

其中，服务定义包含两个部分，以“---”行分隔。

第一部分是请求消息的定义，请求包含六个joint的 float64 字段。

第二部分包含服务响应。响应只包含一个字段time_elapsed。该time_elapsed字段具有持续时间类型，并且负责指示arm执行移动所花费的时间。

2.修改CMakeLists.txt

下面给出一般CMakeLists.txt构建方法：

## 找到catkin宏和库
## 如果使用了像find_package（catkin REQUIRED COMPONENTS xyz）这样的COMPONENTS列表，还可以找到其他catkin包
find_package(catkin REQUIRED COMPONENTSstd_msgsmessage_generationcontroller_manager
)## 系统依赖关系可以在CMake的约定中找到
# find_package(Boost REQUIRED COMPONENTS system)
################################################
## Declare ROS messages, services and actions ##
################################################## 声明和构建消息、服务或操作## 在“msg”文件夹中生成消息
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )## 在“srv”文件夹中生成服务
#add_service_files(
#   FILES
#   GoToPosition.srv
#)## 在“action”文件夹中生成action
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )## 使用此处列出的任何依赖项生成添加的消息和服务
#generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
#)

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################## 在此声明和构建动态重新配置参数## 在“cfg”文件夹中生成动态重新配置参数
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## catkin_package宏为包生成cmake配置文件
##声明要传递给依赖项目的东西
## INCLUDE_DIRS：如果程序包包含头文件，取消注释
## LIBRARIES：库
## CATKIN_DEPENDS：依赖catkin_packages的项目需要
## DEPENDS：依赖项目也需要该项目的系统依赖catkin_package（
#  INCLUDE_DIRS include
#  LIBRARIES simple_arm
#  CATKIN_DEPENDS other_catkin_pkg
#  DEPENDS system_lib
)

###########
## Build ##
############# 指定头文件的其他位置
## 包(package)位置应在其他位置之前列出
# include_directories(include)## 声明一个c++库
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/simple_arm.cpp
# )## 添加cmake库的目标依赖项
## 例如，可能需要在从消息生成或动态重新配置的库之前生成代码
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})## 使用catkin_make声明C ++可执行文件，所有软件包都在单个CMake上下文中构建
## 推荐的前缀可确保跨软件包的目标名称不会冲突
# add_executable(${PROJECT_NAME}_node src/simple_arm_node.cpp)## 重命名不带前缀的C ++可执行文件
## 上面推荐的前缀导致目标名称很长，以下将目标重命名为较短的版本，以方便用户使用
## 例如 “ rosrun someones_pkg节点”而不是“ rosrun someones_pkg someones_pkg_node”
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")## 与上面的库一样添加可执行文件的cmake目标依赖项
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})## 指定链接库或可执行目标的库
# target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
# )

#############
## Install ##
############## 所有安装目标都应该使用catkin目标变量
# 详见 http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html## 与setup.py相比，标记可执行脚本（Python等）进行安装，可以选择目标位置
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )## 为安装标记可执行文件和/或库
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )## 标记用于安装的cpp头文件
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )## 标记其他文件以供安装(例如:启动及档案袋等)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
############### 添加基于gtest的cpp测试目标和链接库
# catkin_add_gtest(${PROJECT_NAME}-test test/test_simple_arm.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()## 添加由python nosetests运行的文件夹
# catkin_add_nosetests(test)

根据上面的CMakelist.txt构建方法修改文件为：

cmake_minimum_required(VERSION 2.8.3)project(arm1)find_package(catkin REQUIRED COMPONENTSstd_msgsmessage_generation
)add_service_files(FILESGoToPosition.srv
)generate_messages(DEPENDENCIESstd_msgs
)catkin_package()find_package(roslaunch)foreach(dir config launch meshes urdf)install(DIRECTORY ${dir}/DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})endforeach(dir)

3.修改package.xml

package.xml 负责定义许多软件包的属性，例如软件包的名称，版本号，作者，维护者和依赖项。

现在主要依赖考虑关系。之前已经说过了构建时依赖性和运行时包依赖性。在rosdep搜索这些依赖项时， package.xml 是正在解析的文件。现在添加message_generation和message_runtime依赖。

<package format="2"><name>arm1</name><version>1.0.0</version><description><p>URDF Description package for arm1</p><p>This package contains configuration data, 3D models and launch filesfor arm1 robot</p></description><author>TODO</author><maintainer email="TODO@email.com" /><license>BSD</license><buildtool_depend>catkin</buildtool_depend><build_depend>message_generation</build_depend><depend>roslaunch</depend><depend>robot_state_publisher</depend><depend>rviz</depend><depend>joint_state_publisher</depend><depend>gazebo</depend><depend>message_runtime</depend><depend>xacro</depend><export><architecture_independent /></export></package>

4.构建包

运行命令：

$ cd ~/catkin_ws
$ catkin_make
$ cd devel/lib/python2.7/dist-packages
$ ls

如果成功构建工作区，那么GoToPosition在 devel 目录的深层创建了一个包含新服务模块的python包。可以看到图中arm1即为构建的python服务包。

5.arm_mover节点代码

在scirpt目录新建一个名为 arm_mover的文件，代码如下

#!/usr/bin/env pythonimport math
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from arm1.srv import *def at_goal(pos_joint, goal_joint):tolerance = .05result = abs(pos_joint - goal_joint) <= abs(tolerance)return resultdef move_arm(pos_joints):time_elapsed = rospy.Time.now()for i in range(len(pos_joints)):globals()['pub_j'+str(i+1)].publish(pos_joints[i])while True:joint_state = rospy.wait_for_message('/arm1/joint_states', JointState)result = Truefor i in range(len(pos_joints)):result = at_goal(joint_state.position[i], pos_joints[i]) and resultif result:time_elapsed = joint_state.header.stamp - time_elapsedbreakreturn time_elapsed# 定义joint限制方法
def clamp_at_boundaries(requested_joint):min_joint = rospy.get_param('~min_joint_angle', -2*math.pi)max_joint = rospy.get_param('~max_joint_angle', 2*math.pi)clamped_joint = requested_jointif not min_joint <= requested_joint <= max_joint:clamped_joint = min(max(requested_joint, min_joint), max_joint)rospy.logwarn('j1 is out of bounds, valid range (%s,%s), clamping to: %s',min_joint, max_joint, clamped_joint)return clamped_joint# 处理move请求
def handle_move_request(req):joint1 = req.joint1joint2 = req.joint2joint3 = req.joint3joint4 = req.joint4right_joint = req.right_jointleft_joint = req.left_jointjoints = [joint1, joint2, joint3, joint4, right_joint, left_joint]clamp_joint = [0, 0, 0, 0, 0, 0]for i in range(len(joints)):rospy.loginfo('GoToPositionRequest Received - j%s:%s,', i, joints[i])clamp_joint[i] = clamp_at_boundaries(joints[i])time_elapsed = move_arm(clamp_joint)return GoToPositionResponse(time_elapsed)# 定义mover服务
def mover_service():rospy.init_node('arm_mover')service = rospy.Service('~arm_mover', GoToPosition, handle_move_request)rospy.spin()if __name__ == '__main__':pub_j1 = rospy.Publisher('/arm1/joint1_position_controller/command',Float64, queue_size=10)pub_j2 = rospy.Publisher('/arm1/joint2_position_controller/command',Float64, queue_size=10)pub_j3 = rospy.Publisher('/arm1/joint3_position_controller/command',Float64, queue_size=10)pub_j4 = rospy.Publisher('/arm1/joint4_position_controller/command',Float64, queue_size=10)pub_j5 = rospy.Publisher('/arm1/right_joint_position_controller/command',Float64, queue_size=10)pub_j6 = rospy.Publisher('/arm1/left_joint_position_controller/command',Float64, queue_size=10)try:mover_service()except rospy.ROSInterruptException:pass

完成之后，运行命令将文件改为可执行文件。

$ sudo chmod 777 arm_mover

6.Arm Mover的启动和互动

（1）修改gazebo.launch

要使 arm_mover 节点和随附的 arm_mover 服务与所有其他节点一起启动，需要修改gazebo.launch 。

在文件中添加以下代码并保存：

  <node name="arm_mover" type="arm_mover" pkg="arm1"><rosparam>min_joint_angle: -1.57max_joint_angle: 1.57</rosparam></node>

（2）测试arm_mover服务

运行命令

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roslaunch arm1 gazebo.launch

然后，在新终端中，验证节点和服务是否确实已启动。

$ rosnode list
$ rosservice list

若正常显示则为下图:

接下来可以使用call命令joint移动了。新开一个终端,例如输入命令：

rosservice call /arm_mover/safe_move 1.0 1.0 1.0 1.0 0.2 0.2

效果如下：