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

168
0
2020年12月26日 09时09分

【从零开始的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

 

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服务包。

 

2

 

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

 

若正常显示则为下图:

 

3

 

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

 

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

 

效果如下:

 

5

 

发表评论

后才能评论