【从零开始的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 COMPONENTS
std_msgs
message_generation
controller_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 COMPONENTS
std_msgs
message_generation
)
add_service_files(
FILES
GoToPosition.srv
)
generate_messages(
DEPENDENCIES
std_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 files
for 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 python
import 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 = .05
result = abs(pos_joint - goal_joint) <= abs(tolerance)
return result
def 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 = True
for i in range(len(pos_joints)):
result = at_goal(joint_state.position[i], pos_joints[i]) and result
if result:
time_elapsed = joint_state.header.stamp - time_elapsed
break
return 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_joint
if 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.joint1
joint2 = req.joint2
joint3 = req.joint3
joint4 = req.joint4
right_joint = req.right_joint
left_joint = req.left_joint
joints = [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.57
max_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
效果如下：
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明【从零开始的ROS四轴机械臂控制】（五）- 构建运动控制服务！
