Udacity机器人软件工程师课程笔记（十二）-ROS-编写更复杂的ROS节点(arm_mover节点 和 look_away 节点)
更复杂的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 COMPONENTSstd_msgsmessage_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(FILESGoToPosition.srv
)
这告诉catkin哪些文件生成代码。
最后，确保 generate_messages() 取消注释，如下所示：
generate_messages(DEPENDENCIESstd_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 pythonimport 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 = .05result = 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_j1clamped_j2 = requested_j2min_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_elapsedbreakreturn 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 pythonimport 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 = .05result = 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_j1clamped_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_elapsedbreakreturn 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: 0max_joint_1_angle: 1.57min_joint_2_angle: 0max_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π/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 = Noneself.arm_moving = Falserospy.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: 0max_joint_1_angle: 1.57min_joint_2_angle: 0max_joint_2_angle: 1.57</rosparam></node>
6.Look Away节点代码
#!/usr/bin/env pythonimport 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 = Noneself.arm_moving = Falserospy.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 Falsetolerance = .0005result = abs(coord_1[0] - coord_2[0]) <= abs(tolerance)result = result and abs(coord_1[1] - coord_2[1]) <= abs(tolerance)return resultdef joint_states_callback(self, data):if self.coord_equal(data.position, self.last_position):self.arm_moving = Falseelse:self.last_position = data.positionself.arm_moving = Truedef 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.57msg.joint_2 = 1.57response = 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 pythonimport 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 = Noneself.arm_moving = Falserospy.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 Falsetolerance = .0005result = 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 = Falseelse:self.last_position = data.positionself.arm_moving = Truedef 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.57msg.joint_2 = 1.57response = 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"
Udacity机器人软件工程师课程笔记（十二）-ROS-编写更复杂的ROS节点(arm_mover节点 和 look_away 节点)相关推荐
- Udacity机器人软件工程师课程笔记（二十五) - 使用PID控制四轴飞行器 - 四轴飞行器（四旋翼）模拟器
1.四轴飞行器运动学和动力学模型 在讨论四轴飞行器时,明确定义两个参考坐标系会很有帮助:一个固定的世界坐标系W{W}W和一个牢固地附着到四轴飞行器的质心(CoM)的运动坐标系B{B}B. 假设运动坐标 ...
- Udacity机器人软件工程师课程笔记（二十一) - 对点云进行集群可视化 - 聚类的分割 - K-means|K均值聚类, DBSCAN算法
聚类的分割 1.K-均值聚类 (1)K-均值聚类介绍 k均值聚类算法(k-means clustering algorithm)是一种迭代求解的聚类分析算法,其步骤是随机选取K个对象作为初始的聚类中心 ...
- Udacity机器人软件工程师课程笔记（二）-样本搜索和找回-基于漫游者号模拟器
Robotics Software engineer编程笔记(二) 5.确定漫游者号的行进方向 (1)漫游者号如何确定自己的行进方向? 我们已经有了一个由前置摄像头得到的图像,然后可以通过对图像进行处 ...
- Udacity机器人软件工程师课程笔记（二十四) - 控制（其二） - PID优化，梯度下降算法，带噪声的PID控制
7.非理想情况 (1)积分饱和 到目前为止,我们一直使用的"理想"形式的PID控制器很少用于工业中."时间常数"形式更为常见. 当前说明了理想形式的一些重大缺陷 ...
- Udacity机器人软件工程师课程笔记（二十) - 感知 - 校准，过滤， 分段， RANSAC
校准,过滤, 分段, RANSAC 首先,我们将讨论传感器校准,也就是说,从几何形状,失真和噪声方面校准相机如何看待周围的世界.了解相机的校准对于了解测量数据如何代表实际物理环境至关重要. 之后,我们 ...
- Udacity机器人软件工程师课程笔记（二十二) - 物体识别 - 色彩直方图，支持向量机SVM
物体识别 1.HSV色彩空间 如果要进行颜色检测,HSV颜色空间是当前最常用的. HSV(Hue, Saturation, Value)是根据颜色的直观特性由A. R. Smith在1978年创建的一 ...
- Udacity机器人软件工程师课程笔记（二十九) - 全卷积网络（FCN）
全卷积网络(FCN) 1.全卷积神经网络介绍 FCN对图像进行像素级的分类,从而解决了语义级别的图像分割(semantic segmentation)问题.与经典的CNN在卷积层之后使用全连接层得到固 ...
- Udacity机器人软件工程师课程笔记（二十八) - 卷积神经网络实例 - Fashion-MNIST数据集
1.Fashion-MNIST数据集 Fashion-MNIST数据集包括一个包含60,000个示例的训练集和一个包含10,000个示例的测试集.每个示例是一个28x28灰度图像,与来自以下10个类的 ...
- Udacity机器人软件工程师课程笔记（二十七) - 卷积神经网络（CNN）
1.卷积神经网络介绍 **卷积神经网络(Convolutional Neural Network,CNN)**是一种前馈神经网络,它的人工神经元可以响应一部分覆盖范围内的周围单元,对于大型图像处理有出 ...
- Udacity机器人软件工程师课程笔记（二十三) - 控制（其一）- PID控制及其python实现
控制(Controls) 1.PID控制简介 在工程实际中,应用最为广泛的调节器控制规律为比例.积分.微分控制,简称PID控制,又称PID调节.PID控制器问世至今已有近70年历史,它 以其结构简单. ...
最新文章
- readonly时禁用删除键，readonly按删除键后页面后退解决方案
- tcp 的ack, seq
- 张小龙做微信公众号APP，对自媒体是祸还是福？
- #ifndef的作用
- Paypal Rest Api自定义物流地址（跳过填写物流地址）
- 课堂作业（求几个数的最大值）
- python-学生管理系统--5 统计学生总人数功能
- 六石管理学：头目们为什么要忽略产品质量
- 联想计算机不能使用ghost,联想电脑不能GHOST的解决方法
- mysql的inndob引擎崩溃
- PHP获取中国所有的大学,全国300所大学的BBS论坛.doc
- 网页编程入门应该首先学些什么
- ESP8266通过arduino IED连接巴法云(TCP创客云)
- Mathematica学习(2)-mathematica命令
- 新一代云上基础技术和架构分论坛
- 前端预言 未来前端的发展方向
- 通过Js将ECharts导出为图片
- 华为鸿蒙系统穿戴app,华为应该如何盘活鸿蒙系统？
- 关于声卡驱动安装问题 ghost版本的危害
- java 如何级联删除_Java学习-040-级联删除目录中的文件、目录
热门文章
- php防止网站被镜像,网站被等恶意镜像的解决、反制措施详细教程
- 安装centos 7 桌面
- [maven] 使用问题及思考汇总
- Android窗口管理服务WindowManagerService计算窗口Z轴位置的过程分析
- web.py下获取get参数
- Swift中的问号?和感叹号!
- 军团要塞2正版服务器,专用服务器配置 - Official TF2 Wiki | Official Team Fortress Wiki
- php限制字符输入,.NET_asp.net(c#)限制用户输入规定的字符和数字的代码，一下是这个代码： 只允许 用 - phpStudy...
- 角色转移服务器维护怎么回事,服务器互通及游戏角色转移说明
- linux sed高级用法,sed 高级用法