Rodney - 漫长的自主机器人之路（第四部分）。
5.00/5 (6投票s)
关于 ROS（机器人操作系统）家用机器人的系列文章的第四部分。
引言
Rodney机器人项目是一个业余机器人项目，旨在使用ROS（机器人操作系统）设计和构建一个自主家用机器人。本文是描述该项目的系列文章中的第四篇。
背景
在第一部分中，为了帮助定义我们机器人的需求，我们选择了第一个任务，并将其分解为多个设计目标，以便于管理。
该任务取自文章让我们来构建一个机器人！，内容是
传递消息给…… - 既然机器人将[拥有]识别家庭成员的能力，何不让它具备“消息记录和提醒者”的能力。我可以对机器人说：“机器人，提醒（某人姓名）晚上6点去车站接我。”然后，即使那位家庭成员的手机调成了静音，或者正在听嘈杂的音乐，或者（任何不去车站接我的理由），机器人也可以在屋里四处走动，找到那个人，并把消息告诉他们。
该任务的设计目标是
- 设计目标1：能够使用摄像头环顾四周，搜索人脸，尝试识别看到的人，并为任何被识别的人显示消息
- 设计目标2：面部表情和语音合成。Rodney需要能够传达消息
- 设计目标3：通过远程键盘和/或游戏手柄控制移动
- 设计目标4：增加激光测距仪或类似的测距传感器以辅助导航
- 设计目标5：自主移动
- 设计目标6：任务分配和完成通知
在文章的前几部分中，我们完成了设计目标1和2。在这一部分，我将介绍一个状态机包，并编写两个节点，用于控制机器人的任务和作业。为了开始将所有内容整合起来，我们将添加第二个任务，它会用到设计目标1和2。
一个复杂的计划
smach
当我们最终准备好将所有这些设计目标整合在一起时，将需要一个复杂的系统来排序和控制系统的各个部分。为此，我们将使用一个名为smach的ROS Python库。该软件包的文档可以在ROS Wiki网站上找到：smach。
使用smach，我们可以开发一个分层状态机，为我们添加的每个任务都增加一个较低级别的状态机。
将设计目标1和2粘合在一起
尽管我们的总体目标是我们定义为任务1的内容，但开始研究这种控制机制是件好事。我们可以做的是将设计目标1和2合并成另一个较小的任务（任务2），即在头部移动范围内搜索已识别的人脸，并向机器人识别出的任何人说出问候语。任务2所使用的流程也将在任务1完成时成为其一部分。
为了完成任务2，在本文中，我们将编写两个节点。第一个节点rodney_missions将包含构成任务和作业的状态机代码。第二个节点rodney将用于控制各种任务和作业的启动时间。我们还将借此机会添加一些读取键盘和游戏手柄的功能，这些功能将在设计目标3中使用。
现在我完全意识到，我在这里引入了一个与“任务(missions)”并列的新术语，那就是“作业(jobs)”。作业将是机器人需要执行的任务，但不如一个完整的任务复杂。运行状态机的节点是执行这些“作业”的最佳位置，因为它们可能需要与更复杂的任务相同的资源。例如，任务1的状态机需要请求头部/摄像头的移动，但我们也可能希望能够手动移动头部/摄像头。尽管让两个节点订阅一个主题是可以的，但让两个节点发布同一个主题被认为是糟糕的做法。所以我们将通过让一个节点来执行“任务”和“作业”来避免这种情况。
到目前为止，我一直保持节点名称的通用性，并没有以这个特定机器人的名字来命名它们。这样做是为了让这些节点可以在其他项目中使用，然而，这两个节点将是这个机器人特有的，所以它们以它的名字命名。
状态机
我们将从包含控制机器人不同任务和作业的状态机的包和节点开始。如上所述，smach是一个Python库，所以我们的包将用Python编写。
我们的ROS包的节点名为 rodney_missions，可以在 rodney_missions 文件夹中找到。src 文件夹包含 rodney_missions_node.py 文件，其中包含节点的主要代码。src 文件夹还包含一个名为 missions_lib 的子文件夹，我们添加到Rodney的每个机器人任务都会生成一个Python文件，该文件将包含在此文件夹中。在这里，我们将处理任务2，其代码在 greet_all.py 文件中。
rodney_missions_node.py 文件将包含注册节点、一些辅助代码的代码，还将包含接受每个任务和作业的高级状态机。greet_all.py 文件将包含任务2的子状态机。每当我们向机器人添加一个新任务时，我们都会为该任务添加一个子状态机。
下图展示了我们当前的状态机。
WAITING（等待）状态是一种特殊类型的状态，称为MonitorState（监控状态），它只监控/missions/mission_request主题。当收到该主题的消息时，它会提取请求以及附带的任何参数，然后传递请求数据并转换到PREPARE（准备）状态。
PREPARE（准备）状态将执行任何“作业”请求，这可能需要转换到MOVE_HEAD（移动头部）、DEFAULT_HEAD_POSITION（默认头部位置）或返回到WAITING（等待）状态。如果请求是执行任务2，那么它将转换到子状态机MISSION2（任务2）。
MOVE_HEAD（移动头部）状态是一种特殊类型的状态，称为SimpleActionState（简单动作状态）。如果进入此状态，将发送一个请求，将头部/摄像头移动到给定位置。移动完成后，它将转换到等待状态。
DEFAULT_HEAD_POSITION（默认头部位置）状态也是一个SimpleActionState，但这一次，请求只能是将头部/摄像头移动到默认位置。移动完成后，它将转换到等待状态。
REPORT（报告）状态只是在/missions/mission_complete主题上发送任务完成消息，然后转换到DEFAULT_HEAD_POSITION（默认头部位置）状态。
PREPARE_FOR_MOVEMENT_GRT 状态计算下一个头部位置应该是哪里。如果还有扫描要做，它将转换到 MOVE_HEAD_GRT 状态，否则，如果所有头部位置都已检查过已知人脸，那么它将转换到 GREETING 状态。
MOVE_HEAD_GRT 是一个 SimpleActionState，它将请求移动头部到前一个状态计算出的位置的动作，然后它会转换到 SCAN_FOR_FACES 状态。
SCAN_FOR_FACES（扫描人脸）状态是一个 SimpleActionState（简单动作状态），但这个状态会请求对当前摄像头图像进行已知人脸的扫描。如果识别出任何人脸，其姓名将在结果中返回，并被存储以备后用。该状态随后转换到 PREPARE_FOR_MOVEMENT_GRT（准备移动-问候）状态。
GREETING（问候）状态会请求向所有被识别出的个体说出问候语，然后转换到报告状态。
如果你回顾这些文章的第二部分，我们编写了两个动作客户端来测试face_recognition和head_control节点，在这段代码中，动作客户端被简单动作状态（Simple Action States）所取代。
在我解释代码之前，值得说明一下/missions/mission_request主题中包含的内容。它的类型是std_msgs/String，包含一个Mission（任务）或Job（作业）的ID，并且根据ID的不同，后面跟着零个或多个用“^”字符分隔的参数。
目前，ID和参数如下
- “
M2” 这个ID是执行任务2的请求，并且没有与之关联的参数。 - “
J1”是执行作业1的请求。这个作业是播放提供的wav文件，并将匹配的文本传递给机器人面部以使嘴唇动起来。ID后的第一个参数是wav文件名，第二个参数是匹配的文本。 - “
J2”是执行作业2的请求。该作业是说出所提供的文本，并将匹配的文本传递给机器人面部以驱动嘴唇动画。第一个参数是要说的文本，第二个参数是匹配的文本。请记住，这两者是分开的，因为用于机器人面部的文本可能包含用于机器人面部表情的笑脸符号。 - “
J3”是执行作业3的请求。此作业是移动头部/摄像头的位置。如果摄像头要向上移动，第一个参数将包含字母“u”；如果摄像头要向下移动，则为“d”；如果摄像头不上下移动，则为“-”。如果摄像头要向左移动，第二个参数包含“l”；如果摄像头要向右移动，则为“r”；如果摄像头不左右移动，则为“-”。第一个参数也可以包含字母“c”，表示将头部移动到默认位置。
现在我将简要描述代码，从 rodney_missions_node.py 文件开始。该文件包含一个主函数、一个顶级状态机类、构成该状态机的状态类，以及一个包含许多将被各种状态使用的辅助函数的类。
main 函数将我们的节点注册到ROS，并创建RodneyMissionsNode类的一个实例。
def main(args):
rospy.init_node('rodney_missions_node', anonymous=False)
rospy.loginfo("Rodney missions node started")
rmn = RodneyMissionsNode()
if __name__ == '__main__':
main(sys.argv)
RodneyMissionsNode 的类构造函数设置在节点关闭时调用 ShutdownCallback。然后它订阅 /missions/mission_cancel 主题，并提供回调函数 CancelCallback。接下来，它创建一个辅助类的实例。然后它创建顶层状态机的各个状态，并将它们添加到状态机中。在这个层面上，MISSION 2 子状态机是我们顶层状态机中的一个单一状态。
然后我们创建并启动一个内省服务器。这对于机器人的运行不是必需的，但它允许你运行一个名为 smach_viewer 的工具。这个工具可以帮助调试你的状态机中的任何问题，并且上面的状态机图就是用它自动生成的。
然后构造函数开始执行状态机并将控制权交给ROS。
RodneyMissionsNode 类中还有另外三个函数。
MissionsRequestCB 是当 MonitorState WAITING 状态在 /missions/mission_request 主题上收到消息时调用的函数。它会从消息中提取数据并将其复制到 userdata，这是一个在状态之间传递数据的过程。然后它返回 False，这样状态机就会转换到 PREPARE 状态。
CancelCallback 是当 /missions/mission_cancel 主题上收到消息时调用的回调函数。这将导致我们正在运行的低级状态机被抢占。
ShutdownCallback 是当节点收到ROS的关闭命令时调用的回调函数。
# Top level state machine. The work for each mission is another state machine
# in the 'mission' states
class RodneyMissionsNode:
def __init__(self):
rospy.on_shutdown(self.ShutdownCallback)
# Subscribe to message to cancel missions
self.__cancel_sub = rospy.Subscriber('/missions/mission_cancel', Empty,
self.CancelCallback)
# Create an instance of the missions helper class
self.__missions_helper = MissionsHelper()
# ------------------------- Top level state machine -------------------------
# Create top level state machine
self.__sm = StateMachine(outcomes=['preempted','aborted'],
output_keys=['mission_data'])
with self.__sm:
# Add a state which monitors for a mission to run
StateMachine.add('WAITING',
MonitorState('/missions/mission_request',
String,
self.MissionRequestCB,
output_keys = ['mission']),
transitions={'valid':'WAITING', 'invalid':'PREPARE',
'preempted':'preempted'})
# Add state to prepare the mission
StateMachine.add('PREPARE',
Prepare(self.__missions_helper),
transitions={'mission2':'MISSION2',
'done_task':'WAITING',
'head_default':'DEFAULT_HEAD_POSITION',
'move_head':'MOVE_HEAD'})
# Add the reporting state
StateMachine.add('REPORT',
Report(),
transitions={'success':'DEFAULT_HEAD_POSITION'})
# Set up action goal for deafult head position
default_position_pan, default_position_tilt =
self.__missions_helper.CameraDefaultPos()
head_goal = point_headGoal()
head_goal.absolute = True
head_goal.pan = default_position_pan
head_goal.tilt = default_position_tilt
# Add the default camera position state.
# Which moves the head to the default position
StateMachine.add('DEFAULT_HEAD_POSITION',
SimpleActionState('head_control_node',
point_headAction,
result_cb =
self.__missions_helper.CameraAtDefaultPos,
goal = head_goal),
transitions={'succeeded':'WAITING','preempted':'WAITING',
'aborted':'aborted'})
# Add the move head state
StateMachine.add('MOVE_HEAD',
SimpleActionState('head_control_node',
point_headAction,
goal_slots=['absolute', 'pan', 'tilt']),
transitions={'succeeded':'WAITING', 'preempted':'WAITING',
'aborted':'aborted'},
remapping={'absolute':'user_data_absolute', 'pan':'user_data_pan',
'tilt':'user_data_tilt'})
# ------------------------- Sub State machine for mission 2 ---------------------
# Create a sub state machine for mission 2 - face detection and greeting
self.__sm_mission2 = missions_lib.Mission2StateMachine(self.__missions_helper)
# Now add the sub state machine for mission 2 to the top level one
StateMachine.add('MISSION2',
self.__sm_mission2,
transitions={'complete':'REPORT','preempted':'REPORT',
'aborted':'aborted'})
# -------------------------------------------------------------------------------
# Create and start the introspective server so that we can use smach_viewer
sis = IntrospectionServer('server_name', self.__sm, '/SM_ROOT')
sis.start()
self.__sm.execute()
# Wait for ctrl-c to stop application
rospy.spin()
sis.stop()
# Monitor State takes /missions/mission_request topic and passes the mission
# in user_data to the PREPARE state
def MissionRequestCB(self, userdata, msg):
# Take the message data and send it to the next state in the userdata
userdata.mission = msg.data
# Returning False means the state transition will follow the invalid line
return False
# Callback for cancel mission message
def CancelCallback(self, data):
# If a sub statemachine for a mission is running then request it be preempted
if self.__sm_mission2.is_running():
self.__sm_mission2.request_preempt()
def ShutdownCallback(self):
self.__sm.request_preempt()
# Although we have requested to shutdown the state machine
# it will not happen if we are in WAITING until a message arrives
PREPARE 和 REPORT 状态包含在 rodney_missions_node.py 文件内的独立类中。
Prepare 类包含一个构造函数，它声明了在 PREPARE 之后是哪个状态，传递给它什么数据，并存储传递给它的辅助类的实例。
该类还包含一个 execute 函数，在进入该状态时运行。此函数检查请求消息，执行它可以完成的任何作业，然后转换到 WAITING 状态，或者转换到另一个状态以运行作业或任务。
# The PREPARE state
class Prepare(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['mission2','done_task','head_default','move_head'],
input_keys=['mission'],
output_keys=['mission_data','start',
'user_data_absolute','user_data_pan','user_data_tilt'])
self.__helper_obj = helper_obj
def execute(self, userdata):
# Based on the userdata either change state to the required mission or
# carry out single job userdata.mission contains the mission or single
# job and a number of parameters seperated by '^'
retVal = 'done_task';
# Split into parameters using '^' as the delimiter
parameters = userdata.mission.split("^")
if parameters[0] == 'M2':
# Mission 2 is scan for faces and greet those known, there are no
# other parameters with this mission request
userdata.start = True
retVal = 'mission2'
elif parameters[0] == 'J1':
# Simple Job 1 is play a supplied wav file and move the face lips
# Publish topic for speech wav and robot face animation
self.__helper_obj.Wav(parameters[1], parameters[2])
elif parameters[0] == 'J2':
# Simple Job 2 is to speak the supplied text and move the face lips
# Publish topic for speech and robot face animation
self.__helper_obj.Speak(parameters[1], parameters[2])
elif parameters[0] == 'J3':
# Simple Job 3 is to move the head/camera. This command will only
# be sent in manual mode.
# parameters[1] will either be 'u', 'd', 'c' or '-'
# parameters[2] will either be 'l', 'r' or '-'
# Check for return to default command
if 'c' in parameters[1]:
retVal = 'head_default'
else:
relative_request_pan, relative_request_tilt =
self.__helper_obj.CameraManualMove(parameters[1]+parameters[2])
# Set up user data that will be used for goal in next state
userdata.user_data_absolute = False # This will be a relative move
userdata.user_data_pan = relative_request_pan
userdata.user_data_tilt = relative_request_tilt
retVal = 'move_head'
return retVal
Report 类包含一个构造函数，它声明了 REPORT 之后的状态，并宣告它将在 /missions/mission_complete 主题上发布一条消息。
该类还包含一个 `execute` 函数，在进入状态时运行。这个函数只是发布一条关于 `/missions/mission_complete` 主题的消息。
# The REPORT state
class Report(State):
def __init__(self):
State.__init__(self, outcomes=['success'])
self.__pub = rospy.Publisher('/missions/mission_complete', String, queue_size=5)
def execute(self, userdata):
# Publishes message that mission completed
self.__pub.publish("Mission Complete")
return 'success'
rodney_missions_node.py 文件中的最后一个类是 MissionHelper 类。这个类包含的代码对不同的任务会很有用，并为我们省去了在最终将要编写的各种子状态机中复制代码的麻烦。这些函数将向语音和机器人面部节点发送消息，并计算头部/摄像头的下一个位置。
# Helper class to hold code used by serveral different states
class MissionsHelper():
def __init__(self):
self.__speech_pub_ = rospy.Publisher('/speech/to_speak', voice, queue_size=1)
self.__text_out_pub = rospy.Publisher('/robot_face/text_out', String, queue_size=1)
# Obtain values from the parameter server
# Minium/Maximum range movment of camera
self.__pan_min = rospy.get_param("/servo/index0/pan/min", -(pi/2.0))
self.__pan_max = rospy.get_param("/servo/index0/pan/max", pi/2.0)
self.__tilt_min = rospy.get_param("/servo/index0/tilt/min", -(pi/2.0))
self.__tilt_max = rospy.get_param("/servo/index0/tilt/max", pi/2.0);
# Default position after mission ends
self.__camera_default_pan_position = rospy.get_param("/head/position/pan", 0.0)
self.__camera_default_tilt_position = rospy.get_param("/head/position/tilt", 0.0)
# Step value to move the camera by when searching
self.__pan_step_value = rospy.get_param("/head/view_step/pan", 0.436332)
self.__tilt_step_value = rospy.get_param("/head/view_step/tilt", 0.436332)
# Step value to move the camera in manual mode
self.__manual_pan_step_value = rospy.get_param("/head/manual_view_step/pan", 0.174533)
self.__manual_tilt_step_value = rospy.get_param("/head/manual_view_step/tilt", 0.174533)
# When true and scanning pan angle will increase, otherwise decrease
self.__increase_pan = True
# Position that will be requested to move the head/camera to
self.__position_request_pan = self.__camera_default_pan_position
self.__position_request_tilt = self.__camera_default_tilt_position
def Speak(self, text_to_speak, text_to_display):
voice_msg = voice()
voice_msg.text = text_to_speak
voice_msg.wav = ""
# Publish topic for speech and robot face animation
self.__speech_pub_.publish(voice_msg)
self.__text_out_pub.publish(text_to_display)
def Wav(self, wav_file, text_to_display):
voice_msg = voice()
voice_msg.text = ""
voice_msg.wav = wav_file
# Publish
self.__speech_pub_.publish(voice_msg)
self.__text_out_pub.publish(text_to_display)
# Function to return the camera start position when scanning within head movement range
def CameraToStartPos(self):
# Set the camera position to pan min and tilt min
self.__position_request_pan = self.__pan_min
self.__position_request_tilt = self.__tilt_max
# Set the variable that says which direction the pan is going. Start by incrementing
self.__increase_pan_ = True
return self.__position_request_pan, self.__position_request_tilt
# Function to keep track of position after action to set to default position
def CameraAtDefaultPos(self, userdata, status, result):
if status == GoalStatus.SUCCEEDED:
self.__position_request_pan = self.__camera_default_pan_position
self.__position_request_tilt = self.__camera_default_tilt_position
# Function returns camera default position
def CameraDefaultPos(self):
return self.__camera_default_pan_position, self.__camera_default_tilt_position
# Function to return the next position when scanning within the head movement range.
# Also returns indication if all areas scanned or more left
def CameraToNextPos(self):
all_areas_scanned = False
# Calculate the next position of the head/camera
if self.__increase_pan == True:
if self.__position_request_pan == self.__pan_max:
# Last scan was at the edge, move tilt up and then pan the other way
self.__increase_pan = False
self.__position_request_tilt -= self.__tilt_step_value
if self.__position_request_tilt < self.__tilt_min:
all_areas_scanned = True
else:
self.__position_request_pan += self.__pan_step_value
if self.__position_request_pan > self.__pan_max:
# Moved out of range, put back on max
self.__position_request_pan = self.__pan_max
else:
if self.__position_request_pan == self.__pan_min:
# Last scan was at the edge, move tilt up and then pan the other way
self.__increase_pan = True
self.__position_request_tilt -= self.__tilt_step_value
if self.__position_request_tilt < self.__tilt_min:
all_areas_scanned = True
else:
self.__position_request_pan -= self.__pan_step_value
if self.__position_request_pan < self.__pan_min:
# Moved out of range, put back on min
self.__position_request_pan = self.__pan_min
if all_areas_scanned == True:
# Reset camera/head position to default values
self.__position_request_pan = self.__camera_default_pan_position
self.__position_request_tilt = self.__camera_default_tilt_position
return all_areas_scanned, self.__position_request_pan, self.__position_request_tilt
def CameraManualMove(self, direction):
relative_request_pan = 0.0
relative_request_tilt = 0.0
# Check for up command
if 'd' in direction:
relative_request_tilt = self.__manual_tilt_step_value
if (self.__position_request_tilt + relative_request_tilt) > self.__tilt_max:
# Would move out of range so move to the max position
relative_request_tilt = self.__tilt_max - self.__position_request_tilt
self.__position_request_tilt = self.__tilt_max
else:
# Keep track
self.__position_request_tilt += relative_request_tilt
# Check for down command
if 'u' in direction:
relative_request_tilt = -(self.__manual_tilt_step_value)
if (self.__position_request_tilt + relative_request_tilt) < self.__tilt_min:
# Would move out of range so move to the min position
relative_request_tilt = self.__tilt_min - self.__position_request_tilt
self.__position_request_tilt = self.__tilt_min
else:
# Keep track
self.__position_request_tilt += relative_request_tilt
# Check for left commnand
if 'l' in direction:
relative_request_pan = self.__manual_pan_step_value
if (self.__position_request_pan + relative_request_pan) > self.__pan_max:
# Would move out of range so move to the max
relative_request_pan = self.__pan_max - self.__position_request_pan
self.__position_request_pan = self.__pan_max
else:
# Keep track
self.__position_request_pan += relative_request_pan
# Check for right command
if 'r' in direction:
relative_request_pan = -(self.__manual_pan_step_value)
if (self.__position_request_pan + relative_request_pan) < self.__pan_min:
# Would move out of range so so move to min
relative_request_pan = self.__pan_min - self.__position_request_pan
self.__position_request_pan = self.__pan_min
else:
# Keep track
self.__position_request_pan += relative_request_pan
return relative_request_pan, relative_request_tilt
我们为各种任务编写的每个子状态机都将包含在子文件夹 missions_lib 中。目前，我们只有任务2，这个子状态机的代码保存在 greet_all.py 中。
此文件中的主类是 Mission2StateMachine，它是一个从 StateMachine 父类派生的类。构造函数初始化子状态机，存储一个辅助类的实例，并创建构成此子状态机的每个状态。该类还包含一个回调函数，当人脸识别动作返回其结果时调用该函数。
# Child (derived) class. Parent class is StateMachine
class Mission2StateMachine(StateMachine):
def __init__(self, helper_obj):
StateMachine.__init__(self, outcomes=['complete','preempted','aborted'],
input_keys=['start'])
self.__helper_obj = helper_obj
with self:
# This state will calculate the next head/camera position
StateMachine.add('PREPARE_FOR_MOVEMENT_GRT',
PrepareMovementGeeting(self.__helper_obj),
transitions={'complete':'GREETING','move':'MOVE_HEAD_GRT'},
remapping={'start_in':'start','start_out':'start'})
# This state will call the action to move the head/camera
StateMachine.add('MOVE_HEAD_GRT',
SimpleActionState('head_control_node',
point_headAction,
goal_slots=['absolute','pan','tilt']),
transitions={'succeeded':'SCAN_FOR_FACES',
'preempted':'preempted','aborted':'aborted'},
remapping={'absolute':'user_data_absolute',
'pan':'user_data_pan','tilt':'user_data_tilt'})
# This state will call the action to scan for faces on the image from the camera
StateMachine.add('SCAN_FOR_FACES',
SimpleActionState('face_recognition',
scan_for_facesAction,
result_cb=self.face_recognition_result_cb,
input_keys=['seen_dict_in'],
output_keys=['seen_dict_out']),
remapping={'seen_dict_in':'seen_dict',
'seen_dict_out':'seen_dict'},
transitions={'succeeded':'PREPARE_FOR_MOVEMENT_GRT',
'preempted':'preempted','aborted':'aborted'})
StateMachine.add('GREETING',
Greeting(self.__helper_obj),
transitions={'complete':'complete'})
def face_recognition_result_cb(self, userdata, status, result):
if status == GoalStatus.SUCCEEDED:
# Face recognition action complete
local_dict = userdata.seen_dict_in
if len(result.ids_detected) > 0:
# Recognised faces detected. Have we seen them before or are they new
for idx, val in enumerate(result.ids_detected):
if val not in local_dict:
# Add to dictionary
local_dict[val] = result.names_detected[idx]
# Log who was seen
rospy.loginfo("Greeting: I have seen %s", result.names_detected[idx])
# Update disctionary stored in user data
userdata.seen_dict_out = local_dict
# By not returning anything the state will return with the
# corresponding outcome of the action
同样在 greet_all.py 文件中，有两个类构成了 PREPARE_FOR_MOVEMENT_GRT 和 GREETING 状态。
# PREPARE_FOR_MOVEMENT_GRT State
class PrepareMovementGeeting(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['complete','move'],
input_keys=['start_in'],
output_keys=['start_out','seen_dict',
'user_data_absolute','user_data_pan','user_data_tilt'])
self.__helper_obj = helper_obj
def execute(self, userdata):
# Is this the start of a new mission
if userdata.start_in == True:
userdata.start_out = False
# clear the seen dictionary
userdata.seen_dict = {}
scan_complete = False
# get the camera start position (pan min and tilt max)
position_request_pan, position_request_tilt =
self.__helper_obj.CameraToStartPos()
else:
scan_complete, position_request_pan, position_request_tilt =
self.__helper_obj.CameraToNextPos()
# Set up user data that will be used for goal in next state if not complete
userdata.user_data_absolute = True
userdata.user_data_pan = position_request_pan
userdata.user_data_tilt = position_request_tilt
if scan_complete == True:
next_outcome = 'complete'
else:
next_outcome = 'move'
return next_outcome
# Greeting State
class Greeting(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['complete'],
input_keys=['seen_dict'])
self.__helper_obj = helper_obj
def execute(self, userdata):
# userdata.seen_dict contains a dictionary of ids and names seen
# Construct greeting
greeting = ''
if len(userdata.seen_dict) == 0:
greeting = 'No one recognised'
else:
greeting = 'Hello '
for id, name in userdata.seen_dict.items():
greeting += name + ' '
greeting += 'how are you '
if len(userdata.seen_dict) == 1:
greeting += 'today'
elif len(userdata.seen_dict) == 2:
greeting += 'both'
else:
greeting += 'all'
rospy.loginfo(greeting)
# Speak greeting
self.__helper_obj.Speak(greeting, greeting + ':)')
return 'complete'
随着我们向项目中添加新任务，我们将遵循这个模板，向库中添加子状态机。
顶层控制
rodney 节点将负责机器人的顶层控制。
我们的ROS包的节点名为rodney，可在rodney文件夹中找到。该包包含了所有常见的ROS文件和文件夹，以及一些额外的文件。
config 文件夹包含一个 config.yaml 文件，可用于覆盖一些默认配置值。您可以配置
- 用于在手动运动模式下使机器人前进和后退的游戏手柄轴
- 用于在手动运动模式下使机器人顺时针和逆时针移动的游戏手柄轴
- 用于在手动运动模式下上下移动头部/摄像头的游戏手柄轴
- 在手动移动模式下，用于左右移动头部/摄像头的游戏手柄轴
- 用于选择手动移动模式的游戏手柄按钮
- 用于将头部/摄像头移回默认位置的游戏手柄按钮
- 游戏手柄摇杆死区值
- 当控制器轴达到其最大范围时请求的线速度
- 当控制器轴达到其最大范围时请求的角速度
- 用于增加或减小线速度的斜坡变化率
- 用于增加或减小角速度的斜坡变化率
- 将发出低电量警告的电池电压水平
- 在机器人不活动时，启用/禁用wav文件播放功能
- 一个在机器人不活动时播放的wav文件名列表
- 播放wav文件名时使用的语音列表
launch 文件夹包含两个启动文件，rodney.launch 和 rviz.launch。rodney.launch 文件用于将前四篇文章中涉及的所有配置文件加载到参数服务器，并启动构成机器人项目的所有节点。它与项目中迄今为止使用的启动文件类似，只是现在包含了 rodney_node 和 rodney_missions_node。rviz 是 ROS 的一个 3D 可视化工具，可用于可视化数据，包括机器人位置和姿态。关于 rviz 的文档可在 ROS Wiki 网站上找到。rviz.launch 文件以及 meshes、rviz 和 urdf 文件夹可用于可视化 Rodney。我们将使用 Rodney 的 urdf 模型在一个模拟的 Rodney 机器人上进行一些测试。
下图显示了 Rodney 在 rviz 中的可视化效果。
rodney_control文件夹只是一个方便存放第一部分讨论过的Arduino文件的地方。
sounds 文件夹用于存放系统需要播放的任何wav文件。如何在播放这些文件的同时让机器人面部动起来，在第3部分已经讲过。
include/rodney 和 src 文件夹包含了该包的 C++ 代码。对于这个包，我们有一个 C++ 类，RodneyNode，以及一个包含在 rodney_node.cpp 文件中的 main 例程。
main 例程将我们的节点告知ROS，创建一个节点类的实例，并将节点句柄传递给它。
同样，我们将在一个循环中自己进行一些处理，所以我们不会通过调用 ros::spin 将控制权交给ROS，而是调用 ros::spinOnce 来处理主题的发送和接收。循环将以20Hz的速率维持，这是通过调用 ros::rate 来设置的，而时间控制则通过在循环内调用 r.sleep 来维持。
我们的循环将在 `ros::ok` 调用返回 `true` 时继续，当节点完成关闭时（例如，当你在键盘上按 **Ctrl-c** 时），它将返回 `false`。
在循环中，我们将调用 checkTimers 和 sendTwist，这将在本文后面描述。
int main(int argc, char **argv)
{
ros::init(argc, argv, "rodney");
ros::NodeHandle n;
RodneyNode rodney_node(n);
std::string node_name = ros::this_node::getName();
ROS_INFO("%s started", node_name.c_str());
ros::Rate r(20); // 20Hz
while(ros::ok())
{
rodney_node.sendTwist();
rodney_node.checkTimers();
ros::spinOnce();
r.sleep();
}
return 0;
}
我们类的构造函数首先为类参数设置默认值。对于每个可以通过ROS参数服务器配置的参数，都会调用 `param` 或 `getParam`。这两个调用的区别在于，使用 `param` 时，如果参数服务器中没有该参数，则使用传递给该调用的默认值。
我们接下来订阅节点感兴趣的主题。
- keyboard/keydown - 用于从键盘获取按键。这些按键由远程PC生成，用于在手动模式下控制机器人。
- joy - 用于获取游戏手柄/游戏控制器输入，同样用于从远程PC控制机器人。
- missions/mission_complete - 以便节点被告知当前机器人任务已完成
- main_battery_status - 这将在项目后期用于接收机器人主电池的状态
- demand_vel - 这将在项目后期用于接收自主速度指令。
构造函数中接下来是该节点将发布的主题的声明。
- /robot_face/expected_input - 这个主题在这些文章的第三部分讨论过，用于在机器人脸部下方显示状态。我们将用它来显示主电池的状态。
- /missions/mission_request - 这将用于将请求的任务和作业传递给状态机节点。
- /missions/mission_cancel - 这个可以用来取消当前正在进行的任务。
- /missions/mission_acknowledge - 这将在项目后期用于确认任务的一部分已完成，并继续执行任务的其余部分。
- /cmd_vel - 这将在项目后期用于向负责驱动电机的节点发送速度命令。请求的速度要么来自自主子系统，要么是手动模式下键盘/游戏手柄请求的结果。
- /commands/reset_odometry - 这将在项目后期用于重置机器人的里程计数值。
最后，构造函数设置一个随机生成器种子并获取当前时间。随机数生成器和时间的使用在 checkTimers 方法部分讨论。
// Constructor
RodneyNode::RodneyNode(ros::NodeHandle n)
{
nh_ = n;
joystick_linear_speed_ = 0.0f;
joystick_angular_speed_ = 0.0f;
linear_mission_demand_ = 0.0f;
angular_mission_demand_ = 0.0f;
manual_locomotion_mode_ = false;
linear_set_speed_ = 0.5f;
angular_set_speed_ = 2.5f;
// Obtain any configuration values from the parameter server.
// If they don't exist, use the defaults above
nh_.param("/controller/axes/linear_speed_index", linear_speed_index_, 0);
nh_.param("/controller/axes/angular_speed_index", angular_speed_index_, 1);
nh_.param("/controller/axes/camera_x_index", camera_x_index_, 2);
nh_.param("/controller/axes/camera_y_index", camera_y_index_, 3);
nh_.param("/controller/buttons/manual_mode_select", manual_mode_select_, 0);
nh_.param("/controller/buttons/default_camera_pos_select", default_camera_pos_select_, 1);
nh_.param("/controller/dead_zone", dead_zone_, 2000);
nh_.param("/teleop/max_linear_speed", max_linear_speed_, 3.0f);
nh_.param("/teleop/max_angular_speed", max_angular_speed_, 3.0f);
nh_.param("/motor/ramp/linear", ramp_for_linear_, 5.0f);
nh_.param("/motor/ramp/angular", ramp_for_angular_, 5.0f);
nh_.param("/battery/warning_level", voltage_level_warning_, 9.5f);
nh_.param("/sounds/enabled", wav_play_enabled_, false);
// Obtain the filename and text for the wav files that can be played
nh_.getParam("/sounds/filenames", wav_file_names_);
nh_.getParam("/sounds/text", wav_file_texts_);
// Subscribe to receive keyboard input, joystick input,
// mission complete and battery state
key_sub_ = nh_.subscribe("keyboard/keydown", 5, &RodneyNode::keyboardCallBack, this);
joy_sub_ = nh_.subscribe("joy", 1, &RodneyNode::joystickCallback, this);
mission_sub_ = nh_.subscribe("/missions/mission_complete", 5,
&RodneyNode::completeCallBack, this);
battery_status_sub_ = nh_.subscribe("main_battery_status", 1,
&RodneyNode::batteryCallback, this);
// The cmd_vel topic below is the command velocity message to the motor driver.
// This can be created from either keyboard or game pad input when in manual mode
// or from this subscribed topic when in autonomous mode.
// It will probably be remapped from the navigation stack
demand_sub_ = nh_.subscribe("demand_vel", 5, &RodneyNode::motorDemandCallBack, this);
// Advertise the topics we publish
face_status_pub_ = nh_.advertise<std_msgs::String>("/robot_face/expected_input", 5);
mission_pub_ = nh_.advertise<std_msgs::String>("/missions/mission_request", 10);
cancel_pub_ = nh_.advertise<std_msgs::Empty>("/missions/mission_cancel", 5);
ack_pub_ = nh_.advertise<std_msgs::Empty>("/missions/acknowledge", 5);
twist_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
reset_odom_ = nh_.advertise<std_msgs::Empty>("/commands/reset_odometry", 1);
battery_low_count_ = 0;
mission_running_ = false;
// Seed the random number generator
srand((unsigned)time(0));
last_interaction_time_ = ros::Time::now();
}
现在我将简要描述构成该类的函数。
当在 Joy 主题上收到消息时，会调用 joystickCallback。来自游戏手柄/游戏控制器的数据可用于在手动模式下移动机器人和移动头部/摄像头。
来自操纵杆的数据分为两个数组，一个包含每个轴的当前位置，另一个包含按钮的当前状态。使用哪个轴和哪个按钮可以通过在参数服务器中设置索引值来配置。
该函数首先读取控制机器人角速度和线速度的轴。将这些值与死区值进行比较，该值决定了轴必须移动多少才能用于控制机器人。然后，将来自控制器的值转换为可用于线速度和速度需求的值。这意味着从控制器接收到的最大可能值将导致对机器人最高速度的需求。这些值被存储起来，并将在 sendTwist 方法中使用。
接下来，读取用于在手动模式下控制头部/摄像头移动的轴，同样，对该值应用了一个死区。如果机器人处于手动移动模式，这些值将作为“J3”作业发送到rondey_mission_node。
接下来检查按钮值。同样，每个功能使用的按钮索引可以配置。一个按钮用于将机器人置于手动移动模式，如果机器人任务当前正在运行，则会导致取消任务的请求。第二个按钮用作将头部/摄像头快速返回到默认位置的方式。
void RodneyNode::joystickCallback(const sensor_msgs::Joy::ConstPtr& msg)
{
float joystick_x_axes;
float joystick_y_axes;
// manual locomotion mode can use the joystick/game pad
joystick_x_axes = msg->axes[angular_speed_index_];
joystick_y_axes = msg->axes[linear_speed_index_];
// Check dead zone values
if(abs(joystick_x_axes) < dead_zone_)
{
joystick_x_axes = 0;
}
if(abs(joystick_y_axes) < dead_zone_)
{
joystick_y_axes = 0;
}
// Check for manual movement
if(joystick_y_axes != 0)
{
joystick_linear_speed_ = -(joystick_y_axes*(max_linear_speed_/(float)MAX_AXES_VALUE_));
last_interaction_time_ = ros::Time::now();
}
else
{
joystick_linear_speed_ = 0;
}
if(joystick_x_axes != 0)
{
joystick_angular_speed_ =
-(joystick_x_axes*(max_angular_speed_/(float)MAX_AXES_VALUE_));
last_interaction_time_ = ros::Time::now();
}
else
{
joystick_angular_speed_ = 0;
}
// Now check the joystick/game pad for manual camera movement
joystick_x_axes = msg->axes[camera_x_index_];
joystick_y_axes = msg->axes[camera_y_index_];
// Check dead zone values
if(abs(joystick_x_axes) < dead_zone_)
{
joystick_x_axes = 0;
}
if(abs(joystick_y_axes) < dead_zone_)
{
joystick_y_axes = 0;
}
if(manual_locomotion_mode_ == true)
{
if((joystick_x_axes != 0) || (joystick_y_axes != 0))
{
std_msgs::String mission_msg;
mission_msg.data = "J3^";
if(joystick_y_axes == 0)
{
mission_msg.data += "-^";
}
else if (joystick_y_axes > 0)
{
mission_msg.data += "u^";
}
else
{
mission_msg.data += "d^";
}
if(joystick_x_axes == 0)
{
mission_msg.data += "-";
}
else if (joystick_x_axes > 0)
{
mission_msg.data += "r";
}
else
{
mission_msg.data += "l";
}
mission_pub_.publish(mission_msg);
last_interaction_time_ = ros::Time::now();
}
}
// Button on controller selects manual locomotion mode
if(msg->buttons[manual_mode_select_] == 1)
{
if(mission_running_ == true)
{
// Cancel the ongoing mission
std_msgs::Empty empty_msg;
cancel_pub_.publish(empty_msg);
}
// Reset speeds to zero
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = 0.0f;
manual_locomotion_mode_ = true;
last_interaction_time_ = ros::Time::now();
}
// Button on controller selects central camera position
if((manual_locomotion_mode_ == true) &&
(msg->buttons[default_camera_pos_select_] == 1))
{
std_msgs::String mission_msg;
mission_msg.data = "J3^c^-";
mission_pub_.publish(mission_msg);
last_interaction_time_ = ros::Time::now();
}
}
当在 keyboard/keydown 主题上收到消息时，会调用 keyboardCallBack。按键可用于在手动模式下移动机器人和移动头部/摄像头。
消息中的数据会被检查，看它是否对应我们感兴趣的按键。
数字键用于选择机器人任务。目前，我们只对任务2感兴趣。
“A”键用于通过在 /missions/missions_acknowledge 主题上发送消息来确认任务的一个部分。
“C”键用于请求取消当前任务，这是通过在 /missions/mission_cancel 主题上发送一条消息来完成的。
如果机器人处于手动移动模式，则使用“D”键将头部/摄像头移回默认位置。
“M”键用于将机器人置于手动移动模式。如果当前有任务正在进行，也会发送一个取消任务的请求。
“R”键用于通过在 /command/reset_odometry 主题上发送消息来重置机器人的里程计值。
键盘数字小键盘用于在手动移动模式下控制机器人的移动。例如，按键“1”将导致反向的线速度加上逆时针方向的角速度。速度的大小由 linear_set_speed_ 和 angular_set_speed_ 变量的当前值设定。这些值可以通过使用数字小键盘上的“+”、“-”、“*”和“/”键来增加或减少。“+”键将使机器人线速度增加10%，而“-”键将使线速度减少10%。“*”使角速度增加10%，“/”键使角速度减少10%。
空格键将使机器人停止移动。
线速度和角速度的概念将在描述Twist消息时讨论。但基本上，机器人不包含可转向的轮子，所以方向的改变将通过请求两个电机不同的速度和/或方向来实现。所需的转向量将由角速度设定。
上/下/左和右键用于在手动模式下移动头部/摄像头。
void RodneyNode::keyboardCallBack(const keyboard::Key::ConstPtr& msg)
{
// Check for any keys we are interested in
// Current keys are:
// 'Space' - Stop the robot from moving if in manual locomotion mode
// 'Key pad 1 and Num Lock off' - Move robot forwards and
// counter-clockwise if in manual locomotion mode
// 'Key pad 2 and Num Lock off' - Move robot backwards if in manual locomotion mode
// 'Key pad 3 and Num Lock off' - Move robot backwards and
// clockwise if in manual locomotion mode
// 'Key pad 4 and Num Lock off' - Move robot counter-clockwise
// if in manual locomotion mode
// 'Key pad 6 and Num Lock off' - Move robot clockwise if in manual locomotion mode
// 'Key pad 7 and Num Lock off' - Move robot forwards and
// counter-clockwise if in manual locomotion mode
// 'Key pad 8 and Num Lock off' - Move robot foward if in manual locomotion mode
// 'Key pad 9 and Num Lock off' - Move robot forwards and
// clockwise if in manual locomotion mode
// 'Up key' - Move head/camera down in manual mode
// 'Down key' - Move head/camera up in manual mode
// 'Right key' - Move head/camera right in manual mode
// 'Left key' - Move head/camera left in manual mode
// 'Key pad +' - Increase linear speed by 10% (speed when using keyboard for teleop)
// 'Key pad -' - Decrease linear speed by 10% (speed when using keyboard for teleop)
// 'Key pad *' - Increase angular speed by 10% (speed when using keyboard for teleop)
// 'Key pad /' - Decrease angular speed by 10% (speed when using keyboard for teleop)
// '1' to '9' - Run a mission (1 -9)
// 'a' or 'A' - Some missions require the user to send an acknowledge
// 'c' or 'C' - Cancel current mission
// 'd' or 'D' - Move head/camera to the default position in manual mode
// 'm' or 'M' - Set locomotion mode to manual
// 'r' or 'R' - Reset the odometry
// Check for a number key (not key pad) with modifiers apart from num lock is allowed
if(((msg->code >= keyboard::Key::KEY_1) && (msg->code <= keyboard::Key::KEY_9)) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Start a mission
std_msgs::String mission_msg;
mission_msg.data = "M" + std::to_string(msg->code-keyboard::Key::KEY_0);
mission_pub_.publish(mission_msg);
mission_running_ = true;
manual_locomotion_mode_ = false;
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_c) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
// 'c' or 'C', cancel mission if one is running
if(mission_running_ == true)
{
std_msgs::Empty empty_msg;
cancel_pub_.publish(empty_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_a) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
// 'a' or 'A', acknowledge a mission step
if(mission_running_ == true)
{
std_msgs::Empty empty_msg;
ack_pub_.publish(empty_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_d) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
// 'd' or 'D', Move camera to default position
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^c^-";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_m) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
// 'm' or 'M', set locomotion mode to manual
// (any missions going to auto should set manual_locomotion_mode_ to false)
// When in manual mode user can teleop Rodney with keyboard or joystick
if(mission_running_ == true)
{
// Cancel the ongoing mission
std_msgs::Empty empty_msg;
cancel_pub_.publish(empty_msg);
}
// Reset speeds to zero
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = 0.0f;
manual_locomotion_mode_ = true;
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_r) &&
((msg->modifiers & ~RodneyNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
// 'r' or 'R', reset odometry command
std_msgs::Empty empty_msg;
reset_odom_.publish(empty_msg);
}
else if((msg->code == keyboard::Key::KEY_KP1) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 1 on keypad without num lock
// If in manual locomotion mode, this is an indication
// to move backwards and counter-clockwise by the current set speeds
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = -linear_set_speed_;
keyboard_angular_speed_ = -angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP2) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 2 on keypad without num lock
// If in manual locomotion mode this is an indication
// to move backwards by the current linear set speed
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = -linear_set_speed_;
keyboard_angular_speed_ = 0.0f;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP3) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 3 on keypad without num lock
// If in manual locomotion mode this is an indication
// to move backwards and clockwise by the current set speeds
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = -linear_set_speed_;
keyboard_angular_speed_ = angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP4) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 4 on keypad without num lock
// If in manual locomotion mode this is an indication
// to turn counter-clockwise (spin on spot) by the current angular set speed
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP6) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 6 on keypad without num lock
// If in manual locomotion mode this is an indication
// to turn clockwise (spin on spot) by the current angular set speed
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = 0.0f;
keyboard_angular_speed_ = -angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP7) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 7 on keypad without num lock
// If in manual locomotion mode this is an indication
// to move forwards and counter-clockwise by the current set speeds
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = linear_set_speed_;
keyboard_angular_speed_ = angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP8) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 8 on keypad without num lock
// If in manual locomotion mode this is an indication
// to move forward by the current linear set speed
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = linear_set_speed_;
keyboard_angular_speed_ = 0.0f;
}
last_interaction_time_ = ros::Time::now();
}
else if((msg->code == keyboard::Key::KEY_KP9) &&
((msg->modifiers & keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 9 on keypad without num lock
// If in manual locomotion mode this is an indication
// to move forwards and clockwise by the current set speeds
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_ = linear_set_speed_;
keyboard_angular_speed_ = -angular_set_speed_;
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_SPACE)
{
// Space key
// If in manual locomotion stop the robot movment
if(manual_locomotion_mode_ == true)
{
keyboard_linear_speed_= 0.0f;
keyboard_angular_speed_ = 0.0f;
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_PLUS)
{
// '+' key on num pad
// If in manual locomotion increase linear speed by 10%
if(manual_locomotion_mode_ == true)
{
linear_set_speed_ += ((10.0/100.0) * linear_set_speed_);
ROS_INFO("Linear Speed now %f", linear_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_MINUS)
{
// '-' key on num pad
// If in manual locomotion decrease linear speed by 10%
if(manual_locomotion_mode_ == true)
{
linear_set_speed_ -= ((10.0/100.0) * linear_set_speed_);
ROS_INFO("Linear Speed now %f", linear_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_MULTIPLY)
{
// '*' key on num pad
// If in manual locomotion increase angular speed by 10%
if(manual_locomotion_mode_ == true)
{
angular_set_speed_ += ((10.0/100.0) * angular_set_speed_);
ROS_INFO("Angular Speed now %f", angular_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_KP_DIVIDE)
{
// '/' key on num pad
// If in manual locomotion decrease angular speed by 10%
if(manual_locomotion_mode_ == true)
{
angular_set_speed_ -= ((10.0/100.0) * angular_set_speed_);
ROS_INFO("Angular Speed now %f", angular_set_speed_);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_UP)
{
// Up Key
// This is a simple job not a mission - move the head/camera down
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^d^-";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_DOWN)
{
// Down Key
// This is a simple job not a mission - move the head/camera up
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^u^-";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_LEFT)
{
// Left key
// This is a simple job not a mission - move the head/camera left
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^-^l";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else if(msg->code == keyboard::Key::KEY_RIGHT)
{
// Right Key
// This is a simple job not a mission - move the head/camera right
if(manual_locomotion_mode_ == true)
{
std_msgs::String mission_msg;
mission_msg.data = "J3^-^r";
mission_pub_.publish(mission_msg);
}
last_interaction_time_ = ros::Time::now();
}
else
{
;
}
}
当在 `main_battery_status` 主题上收到消息时，会调用 `batteryCallback` 函数。此主题的消息类型为 sensor_msgs/BatteryState，其中包含大量电池信息。目前，我们只对电池电压水平感兴趣。
回调函数将发布一条消息，其中包含电量良好或不足的指示以及电池电压值。此消息发布在 /robot_face/expected_input 主题上，因此将显示在机器人动画脸的下方。
电池被视为电量不足的阈值可以通过使用参数服务器进行配置。如果电压低于此值，除了在动画面部下方显示警告外，每隔5分钟还会发送一个请求，要求机器人说出低电量警告。此请求将以“J2”的ID发送到rodney_mission_node。第一个参数是要说的文本，第二个参数是动画面部应该用于其显示的文本。这包括“:(”笑脸，以便机器人面部看起来悲伤。
// Callback for main battery status
void RodneyNode::batteryCallback(const sensor_msgs::BatteryState::ConstPtr& msg)
{
// Convert float to string with two decimal places
std::stringstream ss;
ss << std::fixed << std::setprecision(2) << msg->voltage;
std::string voltage = ss.str();
std_msgs::String status_msg;
// Publish battery voltage to the robot face
// However the '.' will be used by the face to change the expression to neutral
// so we will replace with ','
replace(voltage.begin(), voltage.end(), '.', ',');
if(msg->voltage > voltage_level_warning_)
{
status_msg.data = "Battery level OK ";
battery_low_count_ = 0;
}
else
{
// If the battery level goes low we wait a number of messages
// to confirm it was not a dip as the motors started
if(battery_low_count_ > 1)
{
status_msg.data = "Battery level LOW ";
// Speak warning every 5 minutes
if((ros::Time::now() - last_battery_warn_).toSec() > (5.0*60.0))
{
last_battery_warn_ = ros::Time::now();
std_msgs::String mission_msg;
mission_msg.data = "J2^battery level low^Battery level low:(";
mission_pub_.publish(mission_msg);
}
}
else
{
battery_low_count_++;
}
}
status_msg.data += voltage + "V";
face_status_pub_.publish(status_msg);
}
当在 /missions/mission_complete 主题上收到消息时，会调用 completeCallBack 函数。通过将 missions_running_ 设置为 false，来表示机器人不再运行任务。
void RodneyNode::completeCallBack(const std_msgs::String::ConstPtr& msg)
{
mission_running_ = false;
last_interaction_time_ = ros::Time::now();
}
当在 demand_vel 主题上收到消息时，会调用 motorDemandCallBack 函数。
机器人的运动将是手动的或自主的，这个节点负责使用来自键盘或游戏手柄在手动模式下创建的需求，或者来自自主子系统的需求。这个回调函数只是存储来自自主子系统的线性和角速度需求。
// Callback for when motor demands received in autonomous mode
void RodneyNode::motorDemandCallBack(const geometry_msgs::Twist::ConstPtr& msg)
{
linear_mission_demand_ = msg->linear.x;
angular_mission_demand_ = msg->angular.z;
}
sendTwist 函数是在我们的循环中从 main 调用的函数之一。它决定应使用哪个输入来请求实际的电动机需求，即游戏手柄、键盘或自主子系统。所选的需求会通过消息发布在 cmd_vel 主题上。请注意，需求总是会被发布，因为系统保持恒定需求速率是正常做法。如果需求没有发送，那么控制电机的系统部分可以出于安全考虑将其关闭。
该消息的类型为 geometry_msgs/Twist，包含两个向量，一个用于线速度（米/秒），一个用于角速度（弧度/秒）。每个向量都给出了三个维度的速度，现在对于线性速度，我们只使用x方向，对于角速度，只使用围绕z方向的速度。使用这种消息类型可能看起来有点小题大做，但它确实意味着我们可以在项目后期利用现有的路径规划和避障软件。发布这个主题也意味着我们可以在 Gazebo 中模拟我们机器人的运动。Gazebo是一个机器人仿真工具，我们将在本文的这一部分稍后使用它来测试我们的一些代码。
为了将速度平滑地提升到目标需求，回调函数利用了两个辅助函数，rampedTwist 和 rampedVel。我们使用这些函数来平滑目标速度，以防止因试图以一个大的速度阶跃变化移动机器人而可能发生的打滑和抖动。这两个辅助函数中的代码基于 O'Reilly 出版的《Programming Robots with ROS》一书中的 Python 代码。
void RodneyNode::sendTwist(void)
{
geometry_msgs::Twist target_twist;
// If in manual locomotion mode use keyboard or joystick data
if(manual_locomotion_mode_ == true)
{
// Publish message based on keyboard or joystick speeds
if((keyboard_linear_speed_ == 0) && (keyboard_angular_speed_ == 0))
{
// Use joystick values
target_twist.linear.x = joystick_linear_speed_;
target_twist.angular.z = joystick_angular_speed_;
}
else
{
// use keyboard values
target_twist.linear.x = keyboard_linear_speed_;
target_twist.angular.z = keyboard_angular_speed_;
}
}
else
{
// Use mission demands (autonomous)
target_twist.linear.x = linear_mission_demand_;
target_twist.angular.z = angular_mission_demand_;
}
ros::Time time_now = ros::Time::now();
// Ramp towards are required twist velocities
last_twist_ = rampedTwist(last_twist_, target_twist, last_twist_send_time_, time_now);
last_twist_send_time_ = time_now;
// Publish the Twist message
twist_pub_.publish(last_twist_);
}
//---------------------------------------------------------------------------
geometry_msgs::Twist RodneyNode::rampedTwist(geometry_msgs::Twist prev,
geometry_msgs::Twist target,
ros::Time time_prev, ros::Time time_now)
{
// Ramp the angular and linear values towards the tartget values
geometry_msgs::Twist retVal;
retVal.angular.z = rampedVel(prev.angular.z, target.angular.z,
time_prev, time_now, ramp_for_angular_);
retVal.linear.x = rampedVel(prev.linear.x, target.linear.x,
time_prev, time_now, ramp_for_linear_);
return retVal;
}
//---------------------------------------------------------------------------
float RodneyNode::rampedVel(float velocity_prev, float velocity_target,
ros::Time time_prev, ros::Time time_now,
float ramp_rate)
{
// Either move towards the velocity target or if difference is small jump to it
float retVal;
float sign;
float step = ramp_rate * (time_now - time_prev).toSec();
if(velocity_target > velocity_prev)
{
sign = 1.0f;
}
else
{
sign = -1.0f;
}
float error = std::abs(velocity_target - velocity_prev);
if(error < step)
{
// Can get to target in this within this time step
retVal = velocity_target;
}
else
{
// Move towards our target
retVal = velocity_prev + (sign * step);
}
return retVal;
}
最后一个函数 checkTimers 是在我们的循环中从 main 调用的另一个函数。现在，这里的功能有两个目的。第一个是如果机器人处于非活动状态，也就是说它没有被手动控制，或者它在完成上一个任务后超过15分钟，它会播放一个预先存在的wav文件来提醒你它仍然通电。这个功能可以通过使用参数服务器中的 /sounds/enabled 参数来禁用。
哦，至于这个功能的第二个目的，恐怕是我幽默感的体现，我所有预先存在的wav文件都是科幻机器人的录音。我想如果一个机器人感到无聊，它可能会通过模仿机器人来取乐！“危险，威尔·罗宾逊，危险”。无论如何，如果你不喜欢这个想法，你可以禁用这个功能，或者只是播放别的东西来显示它仍然通电且处于非活动状态。
参数服务器中加载了多个wav文件名和与之匹配的文本句子。当需要播放wav文件时，会生成一个随机数来选择播放哪个wav文件。然后使用ID“J1”发送请求。
void RodneyNode::checkTimers(void)
{
/* Check time since last interaction */
if((wav_play_enabled_ == true) && (mission_running_ == false) &&
((ros::Time::now() - last_interaction_time_).toSec() > (15.0*60.0)))
{
last_interaction_time_ = ros::Time::now();
// Use a random number to pick the wav file
int random = (rand()%wav_file_names_.size())+1;
// This is a simple job not a mission
std_msgs::String mission_msg;
std::string path = ros::package::getPath("rodney");
mission_msg.data = "J1^" + path + "/sounds/" +
wav_file_names_[std::to_string(random)] +
"^" + wav_file_texts_[std::to_string(random)];
mission_pub_.publish(mission_msg);
}
}
游戏手柄节点
在整篇文章中，我们都添加了让机器人通过使用游戏手柄/游戏控制器手动移动的功能。在ROS Wiki网站上有一个名为joy的游戏手柄节点。
然而，我在两台不同的Linux电脑上试用了这个包，发现一直遇到段错误。我没有深入调查问题所在，而是自己写了一个简单的游戏手柄节点。它比ROS网站上的那个简单，因为我没有操心粘滞按钮等问题。
我建议你尝试使用ROS网站上的包，但如果你遇到类似的问题，那么你可以使用我的ROS包，它在 joystick 文件夹中。我已经成功地将它与微软Xbox 360有线控制器一起使用，下面是 joystick_node.cpp 文件的内容。
// Joystick Node. Takes input from a joystick/game pad and
// outputs current state in a sensor_msgs/joy topic.
// See https://linuxkernel.org.cn/doc/Documentation/input/joystick-api.txt
#include <joystick/joystick_node.h>
#include <fcntl.h>
#include <stdio.h>
#include <linux/joystick.h>
// Constructor
Joystick::Joystick(ros::NodeHandle n, std::string device)
{
nh_ = n;
// Advertise the topics we publish
joy_status_pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 5);
js_ = open(device.c_str(), O_RDONLY);
if (js_ == -1)
{
ROS_ERROR("Problem opening joystick device");
}
else
{
int buttons = getButtonCount();
int axes = getAxisCount();
joyMsgs_.buttons.resize(buttons);
joyMsgs_.axes.resize(axes);
ROS_INFO("Joystick number of buttons %d, number of axis %d", buttons, axes);
}
}
// Process the joystick input
void Joystick::process(void)
{
js_event event;
FD_ZERO(&set_);
FD_SET(js_, &set_);
tv_.tv_sec = 0;
tv_.tv_usec = 250000;
int selectResult = select(js_+1, &set_, NULL, NULL, &tv_);
if(selectResult == -1)
{
ROS_ERROR("Error with select joystick call"); // Error
}
else if (selectResult)
{
// Data available
if(read(js_, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
{
// Joystick probably closed
;
}
else
{
switch (event.type)
{
case JS_EVENT_BUTTON:
case JS_EVENT_BUTTON | JS_EVENT_INIT:
// Set the button value
joyMsgs_.buttons[event.number] = (event.value ? 1 : 0);
time_last_msg_ = ros::Time::now();
joyMsgs_.header.stamp = time_last_msg_;
// We publish a button press right away so they are not missied
joy_status_pub_.publish(joyMsgs_);
break;
case JS_EVENT_AXIS:
case JS_EVENT_AXIS | JS_EVENT_INIT:
// Set the axis value
joyMsgs_.axes[event.number] = event.value;
// Only publish if time since last regular message as expired
if((ros::Time::now() - time_last_msg_).toSec() > 0.1f)
{
time_last_msg_ = ros::Time::now();
joyMsgs_.header.stamp = time_last_msg_;
// Time to publish
joy_status_pub_.publish(joyMsgs_);
}
default:
break;
}
}
}
else
{
// No data available, select time expired.
// Publish message to keep anything alive that needs it
time_last_msg_ = ros::Time::now();
joyMsgs_.header.stamp = time_last_msg_;
// Publish the message
joy_status_pub_.publish(joyMsgs_);
}
}
// Returns the number of buttons on the controller or 0 if there is an error.
int Joystick::getButtonCount(void)
{
int buttons;
if (ioctl(js_, JSIOCGBUTTONS, &buttons) == -1)
{
buttons = 0;
}
return buttons;
}
// Returns the number of axes on the controller or 0 if there is an error.
int Joystick::getAxisCount(void)
{
int axes;
if (ioctl(js_, JSIOCGAXES, &axes) == -1)
{
axes = 0;
}
return axes;
}
int main(int argc, char **argv)
{
std::string device;
ros::init(argc, argv, "joystick_node");
// ros::init() parses argc and argv looking for the := operator.
// It then modifies the argc and argv leaving any
// unrecognized command-line parameters for our code to parse.
// Use command line parameter to set the device name of the joystick or use a default.
if (argc > 1)
{
device = argv[1];
}
else
{
device = "/dev/input/js0";
}
ros::NodeHandle n;
Joystick joystick_node(n, device);
std::string node_name = ros::this_node::getName();
ROS_INFO("%s started", node_name.c_str());
// We are not going to use ros::Rate here, the class will use select and
// return when it's time to spin and send any messages on the topic
while(ros::ok())
{
// Check the joystick for an input and process the data
joystick_node.process();
ros::spinOnce();
}
return 0;
}
Using the Code
为了测试我们目前开发的代码，我将在实际的机器人硬件上运行一些测试，但我们也可以在运行于Linux PC上的Gazebo机器人模拟器工具上运行一些测试。在文件夹 rodney/urdf 中，有一个名为 rodney.urdf 的文件，它模拟了Rodney机器人。如何编写一个URDF（统一机器人描述格式）模型本身就需要多篇文章来介绍，但一如既往，ROS Wiki网站上有关于URDF的信息。我的模型远非完美，还需要一些工作，但我们可以在这里用它来测试机器人的运动。所有做这些的文件都包含在 rodney 文件夹和 rodney_sim_control 文件夹中。
在工作站上构建ROS包
在工作站上，除了运行仿真，我们还希望运行键盘和游戏手柄节点，以便我们可以远程控制实际的机器人硬件。
使用以下命令创建一个工作空间
$ mkdir -p ~/test_ws/src
$ cd ~/test_ws/
$ catkin_make
将包 rodney, joystick, rodney_sim_control 和 ros-keyboard (来自 https://github.com/lrse/ros-keyboard) 复制到 ~/test_ws/src 文件夹，然后使用以下命令构建代码
$ cd ~/test_ws/
$ catkin_make
检查构建是否无任何错误地完成。
运行仿真
在 rodney_sim_control 包中，有一个启动文件，它将把机器人模型加载到参数服务器，启动Gazebo并生成一个机器人的仿真。用以下命令启动此文件
$ cd ~/test_ws/
$ source devel/setup.bash
$ roslaunch rodney_sim_control rodney_sim_control.launch
稍等片刻，你应该会看到Rodney的模型出现在一个空旷的世界里。仿真目前处于暂停状态。
在一个新的终端中，加载rodney配置文件并运行rodney节点，使用以下命令
$ cd ~/test_ws $ source devel/setup.bash $ rosparam load src/rodney/config/config.yaml $ rosrun rodney rodney_node
应该会看到一条信息消息，报告节点正在运行。
第一个测试将检验 demand_vel 主题上的消息是否能像来自自主子系统一样控制机器人的运动。
在Gazebo中，点击主屏幕左下角的播放按钮，开始模拟。在一个新的终端中，输入以下内容以在 demand_vel 主题上发送一条消息。
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{linear: {x: 0.5}}'
模拟机器人将以0.5米/秒的速度向前移动。用以下命令反转方向
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{linear: {x: -0.5}}'
您可以使用以下命令停止机器人的移动
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{linear: {x: 0.0}}'
接下来让模拟机器人在原地旋转，使用以下命令
$ rostopic pub -1 /demand_vel geometry_msgs/Twist '{angular: {z: 1.0}}'
用负值重复该命令将导致机器人顺时针旋转，然后用零值停止运动。
接下来，我们将用键盘功能测试移动。
$ cd ~/test_ws
$ source devel/setup.bash
$ rosrun keyboard keyboard
一个标题为“ROS keyboard input”的小窗口应该正在运行。确保此窗口获得焦点，然后按“m”键将机器人置于手动移动模式。
确保未选择“num lock”。
你现在可以使用键盘的数字小键盘来驱动机器人在模拟世界中四处移动。可以使用以下按键来移动机器人。
Key pad 8 - forward
Key pad 2 - reverse
Key pad 4 - rotate anti-clockwise
Key pad 6 - rotate clockwise
Key pad 7 - forward and left
Key pad 9 - forward and right
Key pad 1 - reverse and left
Key pad 3 - reverse and right
Key pad + increase the linear velocity
Key pad - decrease the linear velocity
Key pad * increase the angular velocity
Key pad / decrease the angular velocity
空格键将使机器人停止。
接下来我们可以用游戏手柄控制器测试移动。确保机器人是静止的。在一个新的终端中，输入以下命令
$ cd ~/test_ws/
$ source devel/setup.bash
$ rosrun joystick joystick_node
应该会显示一条消息，表明节点已经启动。使用未更改的 rodney/config/config.yaml 文件中的配置和一个有线的Xbox 360控制器，您可以使用下图中显示的控件来控制模拟机器人。
从Gazebo菜单中，可以向世界中插入其他物体。下面的视频展示了使用Gazebo运行的移动测试。请注意，在视频中，Rodney是一个四轮驱动机器人，此后我更新了模型，实际的机器人是两轮驱动加脚轮。这将在下一篇文章中当我们移动真正的机器人硬件时进行解释。
在Pi上构建ROS包（机器人硬件）
如果尚未完成，请在树莓派上创建一个catkin工作空间并用以下命令初始化它
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
将包 face_recognition, face_recognition_msgs, head_control, pan_tilt, rondey, rodney_missions, servo_msgs, speech 和 ros-keyboard (来自 https://github.com/lrse/ros-keyboard) 复制到 ~/rodney_ws/src 文件夹中。
除非你想把游戏手柄直接连接到机器人上，否则你不需要在机器人硬件上构建游戏手柄包。但你确实需要构建键盘包，因为它包含一个该包独有的消息。我将使用连接到与机器人同一网络的Linux电脑来远程控制它。
使用以下命令构建代码
$ cd ~/rodney_ws/
$ catkin_make
检查构建是否无任何错误地完成。
您还需要编译Arduino代码并将其下载到Nano以控制舵机。
如果尚未完成，你需要训练人脸识别软件，见第二部分。
在机器人上运行代码
现在我们准备好运行我们的代码了。将Arduino连接到USB端口，使用启动文件来启动节点，命令如下。如果系统中没有主节点在运行，启动命令也会启动主节点 roscore。
$ cd ~/rodney_ws
$ source devel/setup.bash
$ roslaunch rodney rodney.launch
在工作站上，运行以下命令以启动键盘节点
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun keyboard keyboard
一个标题为“ROS keyboard input”的小窗口应该正在运行。
我们将在机器人硬件上运行的第一个测试是“任务2”。确保键盘窗口获得焦点，然后按“2”键开始任务。
机器人应该开始移动头部/摄像头，扫描房间寻找已知的人脸。一旦它在头部移动范围内完成了扫描，它要么会报告没有识别到任何人，要么会向它识别到的人打招呼。
下一个测试将检查使用键盘在手动模式下移动头部/摄像头的能力。确保键盘窗口有焦点，然后按“m”键将系统置于手动模式。使用光标键移动头部/摄像头。按“d”键将头部/摄像头返回到默认位置。
下一个测试将检验使用游戏手柄控制器在手动模式下移动头部/摄像头的能力。在工作站的一个新终端中，输入以下命令
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun joystick joystick_node
应该会显示一条消息，表明节点已经启动。使用未更改的 rodney/config/config.yaml 文件中的配置和一个有线的Xbox 360控制器，您可以使用下图中显示的控件来控制机器人的头部/摄像头移动。
对于下一个测试，我们将测试状态指示。在工作站的终端中，输入以下命令
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rostopic pub -1 main_battery_status sensor_msgs/BatteryState '{voltage: 12}'
机器人面部下方的状态应显示“Battery level OK 12,00V”。
在终端中，执行以下命令
$ rostopic pub -1 main_battery_status sensor_msgs/BatteryState '{voltage: 9.4}'
机器人面部下方的状态应显示“9,40V”。
在终端中，再执行以下命令两次
$ rostopic pub -1 main_battery_status sensor_msgs/BatteryState '{voltage: 9.4}'
机器人脸下方的状态应显示“Battery level low 9,40V”，机器人应说出电池电量低的警告，面部表情应为悲伤。
在最后一条消息发出后的5分钟内再次发送该消息。警告不应被说出。
等待5分钟，然后再次发送消息。这一次，口头警告应该会重复。
下一个测试将检查wav文件播放功能。在从键盘或游戏手柄节点发出最后一条命令15分钟后，机器人应该会播放一个随机的wav文件，并随着wav文件播放动画嘴部。
为了帮助调试，这里是当前系统 `rqt_graph` 的输出。图像的完整尺寸副本包含在源zip文件中。
关注点
在本文的这一部分中，我们添加了控制机器人动作的代码，并将设计目标1和2的代码结合起来形成了任务2。
在下一篇文章中，我们将通过添加电机、电机控制器板和驱动该板的软件来完成设计目标3。我们还将讨论构建Rodney所需的所有机器人硬件，包括电路图和所需硬件列表。
历史
- 2018年11月13日：首次发布
- 2018年11月13日：版本2：修复了嵌入式视频
- 2018年12月6日：版本3：修复了 rodney_node.cpp 中的一个bug。游戏手柄的线速度和角速度必须在构造函数中设置为0.0，否则如果没有运行游戏手柄节点，
cmd_vel中会发送错误的速度值。 - 版本4：现在使用弧度而不是度数以符合ROS标准，并使用
SimpleActionStates来控制头部运动和人脸识别过程。