Arm Mover:The Code

先完成上一篇任务。

arm_mover

#!/usr/bin/env python

import math
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from simple_arm.srv import *

def at_goal(pos_j1, goal_j1, pos_j2, goal_j2):
    tolerance = .05
    result = abs(pos_j1 - goal_j1) <= abs(tolerance)
    result = result and abs(pos_j2 - goal_j2) <= abs(tolerance)
    return result

def clamp_at_boundaries(requested_j1, requested_j2):
    clamped_j1 = requested_j1
    clamped_j2 = requested_j2

    min_j1 = rospy.get_param('~min_joint_1_angle', 0)
    max_j1 = rospy.get_param('~max_joint_1_angle', 2*math.pi)
    min_j2 = rospy.get_param('~min_joint_2_angle', 0)
    max_j2 = rospy.get_param('~max_joint_2_angle', 2*math.pi)

    if not min_j1 <= requested_j1 <= max_j1:
        clamped_j1 = min(max(requested_j1, min_j1), max_j1)
        rospy.logwarn('j1 is out of bounds, valid range (%s,%s), clamping to: %s',
                      min_j1, max_j1, clamped_j1)

    if not min_j2 <= requested_j2 <= max_j2:
        clamped_j2 = min(max(requested_j2, min_j2), max_j2)
        rospy.logwarn('j2 is out of bounds, valid range (%s,%s), clamping to: %s',
                      min_j2, max_j2, clamped_j2)

    return clamped_j1, clamped_j2

def move_arm(pos_j1, pos_j2):
    time_elapsed = rospy.Time.now()
    j1_publisher.publish(pos_j1)
    j2_publisher.publish(pos_j2)

    while True:
        joint_state = rospy.wait_for_message('/simple_arm/joint_states', JointState)
        if at_goal(joint_state.position[0], pos_j1, joint_state.position[1], pos_j2):
            time_elapsed = joint_state.header.stamp - time_elapsed
            break

    return time_elapsed

def handle_safe_move_request(req):
    rospy.loginfo('GoToPositionRequest Received - j1:%s, j2:%s',
                   req.joint_1, req.joint_2)
    clamp_j1, clamp_j2 = clamp_at_boundaries(req.joint_1, req.joint_2)
    time_elapsed = move_arm(clamp_j1, clamp_j2)

    return GoToPositionResponse(time_elapsed)

def mover_service():
    rospy.init_node('arm_mover')
    service = rospy.Service('~safe_move', GoToPosition, handle_safe_move_request)
    rospy.spin()

if __name__ == '__main__':
    j1_publisher = rospy.Publisher('/simple_arm/joint_1_position_controller/command',
                                   Float64, queue_size=10)
    j2_publisher = rospy.Publisher('/simple_arm/joint_2_position_controller/command',
                                   Float64, queue_size=10)

    try:
        mover_service()
    except rospy.ROSInterruptException:
        pass

The code: explained

#!/usr/bin/env python

import math
import rospy
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from simple_arm.srv import *

导入的arm_mover模块与simple_arm相同，但两个新导入除外。 即，JointState消息和simple_arm.srv模块。

JointState消息发布到/ simple_arm / joint_states主题，用于监视手臂的位置。

simple_arm包和srv模块是由catkin自动生成的，作为构建过程的一部分。

def at_goal(pos_j1, goal_j1, pos_j2, goal_j2):
    tolerance = .05
    result = abs(pos_j1 - goal_j1) <= abs(tolerance)
    result = result and abs(pos_j2 - goal_j2) <= abs(tolerance)
    return result

如果关节位置接近目标，则此函数返回True。 当从现实世界中的传感器进行测量时，总是会有一些噪音。gazebo模拟器报告的关节位置也是如此。 如果两个关节位置均在目标的0.05弧度范围内，则返回True。

def clamp_at_boundaries(requested_j1, requested_j2):
    clamped_j1 = requested_j1
    clamped_j2 = requested_j2

clamp_at_boundaries()负责执行每个关节的最小和最大关节角度。 如果通过的关节角度超出可操作范围，它们将被“夹紧”到最接近的允许值。

min_j1 = rospy.get_param('~min_joint_1_angle', 0)
max_j1 = rospy.get_param('~max_joint_1_angle', 2*math.pi)
min_j2 = rospy.get_param('~min_joint_2_angle', 0)
max_j2 = rospy.get_param('~max_joint_2_angle', 2*math.pi)

每次调用clamp_at_boundaries()时，都会从参数服务器中检索最小和最大关节角度。 “〜”是私有名称空间限定符，并且表示我们希望得到的参数在该节点的私有名称空间/ arm_mover /（例如〜min_joint_1_angle解析为/ arm_mover / min_joint_1_angle）内。 第二个参数是要返回的默认值，在rospy.get_param()无法从param服务器获取参数的情况下。

if not min_j1 <= requested_j1 <= max_j1:
    clamped_j1 = min(max(requested_j1, min_j1), max_j1)
    rospy.logwarn('j1 is out of bounds, valid range (%s,%s), clamping to: %s',
                  min_j1, max_j1, clamped_j1)

if not min_j2 <= requested_j2 <= max_j2:
    clamped_j2 = min(max(requested_j2, min_j2), max_j2)
    rospy.logwarn('j2 is out of bounds, valid range (%s,%s), clamping to: %s',
                  min_j2, max_j2, clamped_j2)

return clamped_j1, clamped_j2

如果需要，该功能的其余部分只需夹紧关节角度。 如果请求的关节角度超出范围，则会记录警告消息。

def move_arm(pos_j1, pos_j2):
    time_elapsed = rospy.Time.now()
    j1_publisher.publish(pos_j1)
    j2_publisher.publish(pos_j2)

    while True:
        joint_state = rospy.wait_for_message('/simple_arm/joint_states', JointState)
        if at_goal(joint_state.position[0], pos_j1, joint_state.position[1], pos_j2):
            time_elapsed = joint_state.header.stamp - time_elapsed
            break

    return time_elapsed

move_arm()命令手臂移动，返回手臂移动过去的时间量。

注意：在函数中，我们使用rospy.wait_for_message()调用来接收来自/ simple_arm / joint_states主题的联合状态消息。这是阻止函数调用，这意味着直到/ simple_arm / joint_states主题收到消息才会返回。

一般来说，你不应该使用wait_for_message()。为了清楚起见，我们在这里简单地使用它，并且因为handle_safe_move_request()函数正在调用move_arm，该函数要求响应消息作为返回参数传回。下面更多的讨论。

def handle_safe_move_request(req):
    rospy.loginfo('GoToPositionRequest Received - j1:%s, j2:%s',
                   req.joint_1, req.joint_2)
    clamp_j1, clamp_j2 = clamp_at_boundaries(req.joint_1, req.joint_2)
    time_elapsed = move_arm(clamp_j1, clamp_j2)

    return GoToPositionResponse(time_elapsed)

这是服务处理函数。当服务客户端向safe_move服务发送GoToPosition请求消息时，调用此函数。函数参数req的类型是GoToPositionRequest。 服务响应类型为GoToPositionResponse。

这是服务处理函数，只要收到新的服务请求就会调用它。 从该函数返回对服务请求的响应。

注意：move_arm()被阻挡，并且直到手臂完成其移动才会返回。 传入的消息不能被处理，并且在arm执行它的移动命令时，python脚本中没有其他有用的工作可以完成。 虽然这对这个例子没有实际问题，但通常应该避免这种做法。 避免阻塞执行线程的一个好方法是使用Action。 以下是一些有用的文档，描述了最佳使用主题还是服务，还是使用行动。

def mover_service():
    rospy.init_node('arm_mover')
    service = rospy.Service('~safe_move', GoToPosition, handle_safe_move_request)
    rospy.spin()

这里的节点是用名称“arm_mover”初始化的，并且GoToPosition服务是用名称“safe_move”创建的。 如前所述，“〜”限定符标识safe_move意味着属于此节点的私有名称空间。生成的服务名称将为/ arm_mover / safe_move。rospy.Service()调用的第三个参数是在收到服务请求时应该调用的函数。最后，rospy.spin()只是阻塞，直到节点收到关闭请求。未包含此行将导致mover_service()返回，并且脚本完成执行。

if __name__ == '__main__':
    j1_publisher = rospy.Publisher('/simple_arm/joint_1_position_controller/command', Float64, queue_size=10)
    j2_publisher = rospy.Publisher('/simple_arm/joint_2_position_controller/command', Float64, queue_size=10)

    try:
        mover_service()
    except rospy.ROSInterruptException:
        pass

This section of code is similar, to that of simple_mover()。

<上一篇>
<下一篇>