Udacity机器人软件工程师课程笔记（十二）-ROS-编写更复杂的ROS节点(arm_mover节点和 look_away 节点)

最新推荐文章于 2022-12-09 15:02:50 发布

Stan Fu

最新推荐文章于 2022-12-09 15:02:50 发布

阅读量644

点赞数 1

分类专栏：机器人软件工程文章标签： ROS ROS节点人工智能机器人 python

本文链接：https://blog.csdn.net/qq_37266917/article/details/100631621

版权

机器人软件工程专栏收录该内容

35 篇文章 51 订阅

订阅专栏

更复杂的ROS节点

1. Arm_mover节点

为了打好更好的基础，这是在Arm_mover节点还需要学习的内容

自定义消息生成
服务
参数
启动文件

为了理解上述内容，我们将编写另一个名为arm_mover的节点。

（1）Arm Mover的描述

在很多方面， arm_mover 都非常相似simple_mover。就像 simple_mover，它负责指挥机械臂移动。然而，代替简单地命令遵机械臂循预定轨迹，arm_mover 节点提供服务move_arm ，其允许系统中的其他节点发送 movement_commands 。

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

（2）创建新的服务定义

如前所述，与服务的交互包含两个传递的消息。传递给服务的请求以及从服务接收的响应。请求和响应消息类型的定义包含在包含srv根目录下的目录中的.srv文件中。

让我们为 simple_arm 定义一个新服务。我们称之为GoToPosiion。

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

编辑 GoToPosition.srv ，使它包含以下内容：

float64 joint_1
float64 joint_2
---
duration time_elapsed

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

第一部分是请求消息的定义。这里，请求包含两个 float64 字段，每个字段用于一个 simple_arm 关节。

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

注意：定义自定义消息类型非常相似，唯一的区别是消息定义存在于msg包根目录中，具有“ .msg ”扩展名，而不是.srv，并且不包含“—”部分分频器。

可以在此处和此处找到有关创建消息和服务的更多详细信息。

（3）修改CMakeLists.txt

为了让 catkin 生成允许在代码中使用消息的python模块或C ++库，须首先修改simple_arm的 CMakeLists.txt。

$ cd ~/catkin_ws/src/simple_arm/
$ nano CMakeLists.txt

CMake是基于catkin的构建工具，CMakeLists.txt 它只不过是catkin使用的CMake脚本。

首先，确保find_package()宏列表std_msgs和message_generation所需的包。该find_package()宏应如下所示：

find_package(catkin REQUIRED COMPONENTS
        std_msgs
        message_generation
)

正如名称所示的那样，std_msgs包中包含所有基本消息类型，并且message_generation需要为所有支持的语言（cpp，lisp，python，javascript）生成消息库。

注意：
在CMakeLists.txt中，可能还会看到controller_manager列为必需的包。实际上，这个包不是必需的,可以将其从“必需组件”列表中删除。

接下来，取消注释已注释掉的add_service_files():

## Generate services in the 'srv' folder
add_service_files(
   FILES
   GoToPosition.srv
)

这告诉catkin哪些文件生成代码。

最后，确保 generate_messages() 取消注释，如下所示：

generate_messages(
   DEPENDENCIES
   std_msgs  # Or other packages containing msgs
)

这个宏实际上负责生成代码。

文件 CMakeLists.txt 是用于构建软件包的CMake构建系统的输入。任何符合CMake的包都包含一个或多个 CMakeLists.txt 文件，该文件描述了如何构建代码以及将代码安装到何处。用于catkin项目的 CMakeLists.txt 文件是标准的vanilla CMakeLists.txt文件，其中包含一些其他约束。更多关于 CmakeLists.txt 信息请查阅此处。

（4）修改package.xml

现在该 CMakeLists.txt 文件已被覆盖，现在应该在技术上能够构建该项目。但是，还有一个文件需要修改，即 package.xml 。

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

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

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>message_generation</build_depend>

  <run_depend>controller_manager</run_depend>
  <run_depend>effort_controllers</run_depend>
  <run_depend>gazebo_plugins</run_depend>
  <run_depend>gazebo_ros</run_depend>
  <run_depend>gazebo_ros_control</run_depend>
  <run_depend>joint_state_controller</run_depend>
  <run_depend>joint_state_publisher</run_depend>
  <run_depend>robot_state_publisher</run_depend>
  <run_depend>message_runtime</run_depend>
  <run_depend>xacro</run_depend>

其中只有两行作出添加：

  <build_depend>message_generation</build_depend>
  <run_depend>message_runtime</run_depend>

现在已准备好构建包。

有关更多信息package.xml，请查看ROS Wiki。

（5）构建包

如果成功构建工作区，那么GoToPosition在 devel 目录的深层创建了一个包含新服务模块的python包。

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

在这里插入图片描述
在获取新创建的资源后 setup.bash ，新的 simple_arm 软件包现已成为PYTHONPATH环境变量的一部分，并且可以使用。

$ env | grep PYTHONPATH

在这里插入图片描述

（6）创建空的arm_mover节点脚本

创建 arm_mover 节点所采取的步骤与创建 simple_mover 脚本所采取的步骤完全相同，但脚本本身的实际名称除外。

$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ sudo touch arm_mover
$ sudo chmod 777 arm_mover

之前没有提chmod 777 arm_mover命令的解释，其解释在此处。

2.arm_mover相关代码

(1)完整代码

#!/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 *


# 如果关节位置接近目标，则此函数返回True
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

（2）代码解释

#!/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()命令arm移动，返回机械臂在移动时所需要的时间。

注意：
在这个函数中，我们使用了rospy.wait_for_message()调用来接收来自/simple_arm/joint_states主题的JointSate消息。这是阻塞函数调用，这意味着在收到/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)

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

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

注意：
move_arm()被阻塞，在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。

service()调用的第三个参数是在接收到服务请求时应该调用的函数。最后，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

这部分代码类似于simple_mover()。
———————————————————————————————————————————————————————

3.Arm Mover的启动和互动

使用新服务启动项目

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

当启动文件存在时，它们位于catkin包根目录中的启动目录中。simple_arm 的启动文件位于~/catkin_ws/src/simple_arm/launch中。

要使 arm_mover 节点启动，只需添加以下内容：

  <!-- The arm mover node -->
  <node name="arm_mover" type="arm_mover" pkg="simple_arm">
    <rosparam>
      min_joint_1_angle: 0
      max_joint_1_angle: 1.57
      min_joint_2_angle: 0
      max_joint_2_angle: 1.0
    </rosparam>
  </node>

有关启动文件格式的更多信息，请参见此处。

测试新服务

为此，启动 simple_arm ，验证 arm_mover 节点正在运行，并且列出 safe_move 服务:

注意：
roslaunch在重新启动之前，需要确保已退出上一个会话。

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roslaunch simple_arm robot_spawn.launch

报错：

在这里插入图片描述原因是我在编辑robot_spawn.launch文件时，将程序复制错了位置。

解决办法：
在这里插入图片描述
这便是正确的robot_spawn.launch文件中的内容

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

$ rosnode list
$ rosservice list

在这里插入图片描述

假设service（/arm_mover/safe_move）和node（/arm_mover）都按预期显示（如果没有，请检查roscore控制台中的日志），现在可以使用 rosservice 与服务交互。

要查看摄像机图像流，可以使用该命令rqt_image_view
（在此处了解有关rqt和相关工具的更多信息）：

$ rqt_image_view /rgb_camera/image_raw

在这里插入图片描述

调整视图

照相机显示一幅灰色图像。这是意料之中的，因为它是垂直向上的，朝向灰色的天空。

为了将相机指向计数器顶部的编号块，我们需要将关节1和关节2旋转大约 $π / 2$ 弧度。

$ rosservice call /arm_mover/safe_move "joint_1: 1.57
joint_2: 1.57"

报错：Unable to load type

在这里插入图片描述
经查阅，这是由路径问题导致的，解决办法

 source ~/catkin_ws/devel/setup.bash

注意：rosservice call可以tab-complete请求消息，这样就不必担心手工编写它。此外，请确保在两个关节参数之间包含一个换行符。

在输入命令后，应该能够看到手臂移动，并最终停止，报告将机械臂移动到控制台所花费的时间。
在这里插入图片描述
出乎意料的是手臂的最终位置。查看roscore控制台，我们可以非常清楚地看到问题是什么。关节2要求的角度超出安全范围。我们要求1.57弧度，但最大关节角度设置为1.0弧度。

通过在参数服务器上设置max_joint_2_angle，我们应该能够在下一次进行服务调用时将这些块显示出来。要增加关节2的最大角度，可以使用命令rosparam

$ rosparam set /arm_mover/max_joint_2_angle 1.57

现在应该能够移动手臂，使所有块都在摄像机的视野范围内：

rosservice call /arm_mover/safe_move "joint_1: 1.57
joint_2: 1.57"

在这里插入图片描述

4.ROS订阅者（Subscribers）

订阅者使节点能够从主题读取消息，从而允许有用的数据流到节点中。在Python中，ROS订阅者通常具有以下格式，尽管可以使用其他参数和参数:

sub1 = rospy.Subscriber("/topic_name", message_type, callback_function)

“/topic_name”指示订阅服务器应该听哪个主题。
message_type是发布在“/topic_name”上的消息类型。
callback_function是每个传入消息都应该调用的函数的名称。每次接收到消息时，都将其作为参数传递给callback_function。通常，此函数在节点中定义，以便对传入的数据执行有用的操作。注意，与服务处理函数不同，callback_function不需要返回任何东西

5.Look Away节点

要查看订阅服务器的运行情况，需要编写一个名为look_away的节点。look_away节点将订阅/rgb_camera/image_raw主题，该主题包含安装在机械手末端的摄像机的图像数据。每当相机指向一个无意义的图像(在本例中是一个颜色一致的图像)时，回调函数将把手臂移动到有更多信息的地方。

代码中还有一些额外的部分来确保这个过程的顺利执行。

（1）创建空的look_away节点脚本

就像之前创建arm_mover和simple_mover节点一样，可以创建look_away 节点如下：

$ cd ~/catkin_ws/src/simple_arm/scripts
$ sudo touch look_away
$ sudo chmod u+x look_away

（2）故障排除look_away

在某些情况下，look_away在手动运行时正在执行，但在roslaunch中没有自动执行。这通常是一个时间问题。如果look_away在系统完全初始化之前启动，则look_away挂起对safe_move的调用。

jsteinbae为这个问题提供了一个很好的解决方案:

他解决方案是在订阅主题之前将wait_for_message添加到look_away节点。这可以确保在完全初始化gazebo模拟(发布这些主题)之前不会调用回调。

def __init__(self):
    rospy.init_node('look_away')
    self.last_position = None
    self.arm_moving = False

    rospy.wait_for_message('/simple_arm/joint_states', JointState)
    rospy.wait_for_message('/rgb_camera/image_raw', Image)

    self.sub1 = rospy.Subscriber('/simple_arm/joint_states', 
                                 JointState, self.joint_states_callback)
    self.sub2 = rospy.Subscriber('/rgb_camera/image_raw', 
                                 Image, self.look_away_callback)
    self.safe_move = rospy.ServiceProxy('/arm_mover/safe_move', 
                                 GoToPosition)
    rospy.spin()

（3）更新启动文件

和对 arm_mover 节点所做的那样，要 look_away 与其余节点一起启动，需要修改 robot_spawn.launch ，可以在其中找到 ~/catkin_ws/src/simple_arm/launch。并在其中添加以下代码：

  <!-- The look away node -->
  <node name="look_away" type="look_away" pkg="simple_arm"/>

在编辑这个文件时，在 arm_mover 中设置max_joint_2_angle: 1.57将会很有帮助，这样就不需要从命令行再次设置它:

  <!-- The arm mover node -->
  <node name="arm_mover" type="arm_mover" pkg="simple_arm">
    <rosparam>
      min_joint_1_angle: 0
      max_joint_1_angle: 1.57
      min_joint_2_angle: 0
      max_joint_2_angle: 1.57
    </rosparam>
  </node>

6.Look Away节点代码


#!/usr/bin/env python

import math
import rospy
from sensor_msgs.msg import Image, JointState
from simple_arm.srv import *


class LookAway(object):
    def __init__(self):
        rospy.init_node('look_away')

        self.sub1 = rospy.Subscriber('/simple_arm/joint_states', 
                                     JointState, self.joint_states_callback)
        self.sub2 = rospy.Subscriber("rgb_camera/image_raw", 
                                     Image, self.look_away_callback)
        self.safe_move = rospy.ServiceProxy('/arm_mover/safe_move', 
                                     GoToPosition)

        self.last_position = None
        self.arm_moving = False

        rospy.spin()

    def uniform_image(self, image):
        return all(value == image[0] for value in image)

    def coord_equal(self, coord_1, coord_2):
        if coord_1 is None or coord_2 is None:
            return False
        tolerance = .0005
        result = abs(coord_1[0] - coord_2[0]) <= abs(tolerance)
        result = result and abs(coord_1[1] - coord_2[1]) <= abs(tolerance)
        return result

    def joint_states_callback(self, data):
        if self.coord_equal(data.position, self.last_position):
            self.arm_moving = False
        else:
            self.last_position = data.position
            self.arm_moving = True

    def look_away_callback(self, data):
        if not self.arm_moving and self.uniform_image(data.data):
            try:
                rospy.wait_for_service('/arm_mover/safe_move')
                msg = GoToPositionRequest()
                msg.joint_1 = 1.57
                msg.joint_2 = 1.57
                response = self.safe_move(msg)

                rospy.logwarn("Camera detecting uniform image. \
                               Elapsed time to look at something nicer:\n%s", 
                               response)

            except rospy.ServiceException, e:
                rospy.logwarn("Service call failed: %s", e)



if __name__ == '__main__':
    try: 
        LookAway()
    except rospy.ROSInterruptException:
        pass

7.Look Away 节点代码解释

导入语句

#!/usr/bin/env python

import math
import rospy
from sensor_msgs.msg import Image, JointState
from simple_arm.srv import *

导入的模块与之类似simple_arm，除了这次，我们Image将导入消息类型，以便可以使用摄像机数据。
———————————————————————————————————————————————————————

LookAway类和init方法

class LookAway(object):
    def __init__(self):
        rospy.init_node('look_away')

        self.sub1 = rospy.Subscriber('/simple_arm/joint_states', 
                                     JointState, self.joint_states_callback)
        self.sub2 = rospy.Subscriber("rgb_camera/image_raw", 
                                     Image, self.look_away_callback)
        self.safe_move = rospy.ServiceProxy('/arm_mover/safe_move', 
                                     GoToPosition)

        self.last_position = None
        self.arm_moving = False

        rospy.spin()

首先为这个节点定义了一个类，以便更好地跟踪机械臂当前的运动状态和位置历史。与之前的节点定义一样，节点是使用ropsy初始化的。方法末尾使用rospy.spin()进行阻塞，直到节点接收到关闭请求。

第一个订阅者，self.sub1 订阅/simple_arm/joint_states主题。节点只在机械臂不动时检查相机，通过订阅/simple_arm/joint_states，可以跟踪机械臂位置的变化。这个主题的消息类型是JointState，对于每个消息，消息数据都被传递给joint_states_callback函数。

第二个订阅者 self.sub2 订阅该/rgb_camera/image_raw主题。这里的消息类型是Image，并且每条消息都会调用该look_away_callback函数。

A ServiceProxy是rospy如何从节点调用服务。在ServiceProxy这里使用想与服务类定义一起调用该服务的名称创建：在这种情况下/arm_mover/safe_move和GoToPosition。对服务的实际调用将在以下look_away_callback方法中进行。
———————————————————————————————————————————————————————

辅助方法

    def uniform_image(self, image):
        return all(value == image[0] for value in image)

    def coord_equal(self, coord_1, coord_2):
        if coord_1 is None or coord_2 is None:
            return False
        tolerance = .0005
        result = abs(coord_1[0] - coord_2[0]) <= abs(tolerance)
        result = result and abs(coord_1[1] - coord_2[1]) <= abs(tolerance)
        return result

代码中定义了两个辅助方法：uniform_image和coord_equal。

该uniform_image方法将图像作为输入并检查图像中的所有颜色值是否与第一像素的值相同。这会检查图像中的所有颜色值是否相同。

该coord_equal如果坐标方法返回true coord_1，并coord_2具有相同的组件升级到规定的公差。
———————————————————————————————————————————————————————

回调函数

    def joint_states_callback(self, data):
        if self.coord_equal(data.position, self.last_position):
            self.arm_moving = False
        else:
            self.last_position = data.position
            self.arm_moving = True

    def look_away_callback(self, data):
        if not self.arm_moving and self.uniform_image(data.data):
            try:
                rospy.wait_for_service('/arm_mover/safe_move')
                msg = GoToPositionRequest()
                msg.joint_1 = 1.57
                msg.joint_2 = 1.57
                response = self.safe_move(msg)

                rospy.logwarn("Camera detecting uniform image. \
                               Elapsed time to look at something nicer:\n%s", 
                               response)

            except rospy.ServiceException, e:
                rospy.logwarn("Service call failed: %s", e)

当 self.sub1 上接收到消息/simple_arm/joint_states的主题，该消息被传递到在变量data中的joint_states_callback。在joint_states_callback使用coord_equal辅助方法，以检查是否在所提供的当前关节状态data是相同的前面的关节状态，其被存储在self.last_position。

如果当前和先前的关节状态相同（达到指定的公差），则臂已停止移动，因此self.arm_moving标志设置为False。

如果当前和先前的关节状态不同，则手臂仍在移动。在这种情况下，该方法self.last_position使用当前位置数据更新并设置self.arm_moving为True。

在look_away_callback从接收数据/rgb_camera/image_raw的主题。此方法的第一行验证手臂是否移动，并检查图像是否均匀。如果机械臂没有移动且图像是均匀的，则会GoToPositionRequest()创建一条消息并使用该safe_move服务发送，同时移动两个关节角度1.57。

该方法还会记录一条消息，警告相机已检测到统一的图像以及返回更好的图像所用的时间。
———————————————————————————————————————————————————————

8.Look Away：启动和交互

现在可以像以前simple_arm一样启动和交互：

$ cd ~/catkin_ws
$ catkin_make
$ source devel/setup.bash
$ roslaunch simple_arm robot_spawn.launch

请注意，如果在使用roslaunch simple_arm robot_spawn.launch时遇到问题，请尝试脚本文件夹中的safe_spawner.sh脚本。可以在选择的终端中使用$ ./safe_spawner.sh启动。

发送命令后，机械臂应远离灰色的天空，向积木方向移动。
在这里插入图片描述
要查看相机图像流，可以使用与之前相同的命令:

$ rqt_image_view /rgb_camera/image_raw

要检查一切是否按预期工作，请打开一个新的终端，来源devel/setup.bash，然后发送命令，将指针直接指向天空（请注意，消息中的换行符是必要的）：

rosservice call /arm_mover/safe_move "joint_1: 0
joint_2: 0"

在这里插入图片描述

Stan Fu

关注

1
点赞
踩
3

收藏

觉得还不错? 一键收藏
打赏
0
评论
Udacity机器人软件工程师课程笔记（十二）-ROS-编写更复杂的ROS节点(arm_mover节点和 look_away 节点)

更复杂的ROS节点1. Arm_mover节点为了打好更好的基础，这是还需要学习的内容自定义消息生成服务参数启动文件为了理解上述内容，我们将编写另一个名为arm_mover的节点。（1）Arm Mover的描述在很多方面，arm_mover都非常相似simple_mover。就像simple_mover，它负责指挥手臂移动。然而，代替简单地命令臂遵循预定轨迹，arm_move...
复制链接

扫一扫