Rodney - 迟来的自主机器人（第八部分）
5.00/5 (11投票s)
关于 ROS（机器人操作系统）家用机器人系列中的第八部分
引言
Rodney机器人项目是一个业余机器人项目，旨在设计和构建一个使用ROS（机器人操作系统）的自主家用机器人。本文是描述该项目的第八篇文章。
背景
在第一部分中，为了帮助定义我们机器人的需求，我们选择了我们的第一个任务，并将其分解为多个设计目标，以便更易于管理。
这个任务取自文章让我们建造一个机器人！，内容是：给…带个口信 - 由于机器人将[拥有]识别家庭成员的能力，那么能否让它成为“信息传递者和提醒者”。我可以对机器人说“机器人，提醒（人名）晚上6点到车站接我”。然后，即使该家庭成员的手机静音，或者正在听嘈杂的音乐，或者（插入任何不来车站接我的理由），机器人也可以在房子里漫游，找到那个人，并把信息传达给他们。
该任务的设计目标是
- 设计目标1：能够使用摄像头环顾四周，搜索人脸，尝试识别看到的人，并为任何被识别的人显示消息
- 设计目标2：面部表情和语音合成。Rodney需要能够传达消息
- 设计目标3：通过远程键盘和/或游戏手柄控制移动
- 设计目标4：增加激光测距仪或类似的测距传感器以辅助导航
- 设计目标5：自主移动
- 设计目标6：任务分配和完成通知
在上一部分中，我们添加了ROS导航堆栈，使Rodney能够自主移动。我们使用可视化工具rviz设置了机器人要导航到的目标。在这一部分中，我们将添加任务代码，以导航到房屋的不同部分，并在每个位置搜索已知人员以传递消息。要访问的位置将从yaml文件加载。
我们将更新rodney和rodney_missions包，并添加一个新的包，使我们能够将LED和按钮添加到树莓派GPIO（通用输入输出）。
再次回顾Smach
在第四部分中，我们研究了用于创建状态机的smach ROS包。我们创建了一个分层状态机，其中为问候任务（任务2）添加了一个低级状态机。我们编写代码的方式使得通过为任何新任务添加低级状态机来添加新任务应该相对简单。在这一部分中，我们将添加两个新任务作为低级状态机。第一个任务，任务1，将执行我们的“给…带个口信”任务；第二个任务，任务4，将是一个“回家”任务，用于将机器人送回指定位置。
在此之前，我们将研究添加一个双色LED、两个按钮，并更新rodney包以将新的任务数据发送到状态机。
GPIO
虽然我们有触摸屏，但我还是想添加两个按钮来向Rodney输入命令。由于屏幕是机器人头部，并且可以移动，这意味着它可能不适合用户与机器人交互。因此，我们将在机器人硬件上添加两个按钮。每个按钮将有两个功能，执行的功能取决于机器人当前是否正在运行任务。
第一个按钮，即黑色按钮，将执行以下功能。如果没有任务运行并且按下该按钮，机器人头部将移动到适合用户在屏幕上进行输入/输出的位置。如果任务正在运行并且按下该按钮，将发送命令取消当前任务。
第二个按钮，即黄色按钮，将执行以下功能。如果没有任务运行并且按下该按钮，将发出命令运行“回家”任务。此任务将引导机器人导航到已知的主位置。有些任务的状态要求用户确认机器人。例如，当机器人传递口头信息时，用户应该确认收到信息。此确认将是黄色按钮的第二个功能。如果在任务运行时按下它，确认消息将发送到状态机。
除了这两个按钮，我们还将为机器人硬件添加一个双色LED。当机器人没有执行实时任务时，LED将亮绿色；当机器人正在执行任务时，它将亮红色。
我们可以将所需的GPIO代码添加到rodney包中，但为了代码重用，我们将在项目中添加一个新包来处理GPIO。请记住，您可以用多种不同的语言编写ROS代码，由于我们将使用现有的Python库来访问树莓派GPIO，因此我们将使用Python编写这个新节点。
我们的节点ROS包名为pi_io，位于pi_io文件夹中。在这个包中，我们将创建一个ROS服务来改变GPIO输出线的状态。虽然我们之前的文章中没有编写自己的ROS服务，但我们使用了一个现有的服务来启用/禁用LIDAR。我们还将创建一个新的用户定义消息，当其中一个按钮改变状态时将发布该消息。
该包包含所有常见的ROS文件和文件夹。
msg文件夹包含gpio_input.msg文件，其中包含我们用户定义的ROS消息的定义。
uint8 index # GPIO index starting at 0
bool value # GPIO value
该消息包含两个参数。第一个是消息所指GPIO的索引。代码将此索引映射到GPIO引脚。第二个参数是该引脚的当前状态。
srv文件夹包含gpio_output.srv文件，其中包含我们用户定义的ROS服务的定义。服务可以看作是远程过程调用，与我们在之前的文章中介绍的ROS动作不同，服务应该快速返回，因为它们是阻塞的。
uint8 index # GPIO index starting at 0
bool value # GPIO value
---
bool success
该消息包含三个参数。第一个是指示该值所指GPIO的索引。代码将此索引映射到GPIO引脚。第二个参数是要设置该引脚的值。我们的服务将返回一个布尔成功值。对于树莓派，我们无法判断GPIO引脚是否已设置，因此我们总是返回true。对于允许您读取输出引脚反馈的开发板，我们只有在引脚设置为正确值时才返回true。可以通过在“---”行之后不输入任何内容来将服务的返回值设置为空。
src文件夹包含pi_io_node.py文件中的节点Python代码。我现在将简要描述该代码。
main函数向ROS注册我们的节点，并创建一个PiGpioNode类的实例。
def main(args):
rospy.init_node('pi_io_node', anonymous=False)
rospy.loginfo("Rapberry Pi GPIO node started")
piio = PiGpioNode()
try:
rospy.spin()
except KeyboardInterrupt:
print('Shutting down')
if __name__ == '__main__':
main(sys.argv)
PiGpioNode的类构造函数将我们实际使用的GPIO引脚映射到消息和服务中使用的索引值。然后，它调用Pi GPIO Python库，声明我们通过GPIO引脚编号而不是Pi连接器的物理引脚编号来引用GPIO。它设置哪些GPIO引脚是输出，哪些是输入。对于输入，它还声明引脚应该被拉低。当我们的按钮被按下时，它将把线路拉高。
输入引脚还连接到一个事件，当各自的输入变为高电平时，将调用函数Input0HighCallback和Input1HighCallback。
构造函数随后向ROS注册服务和消息主题。我们还创建了两个计时器，用于消除按钮抖动。
class PiGpioNode:
def __init__(self):
self.__output0 = 23 # GPIO Pin 23 is output index 0
self.__output1 = 24 # GPIO Pin 23 is output index 1
self.__input0 = 27 # GPIO Pin 27 is input index 0
self.__input1 = 22 # GPIO Pin 22 is input index 1
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # Use GPIO pin number not the physical pin numbering
GPIO.setup(self.__output0, GPIO.OUT)
GPIO.setup(self.__output1, GPIO.OUT)
GPIO.setup(self.__input0, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(self.__input1, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
# Currently we only publish message when input goes high
GPIO.add_event_detect(self.__input0, GPIO.RISING, callback=self.Input0HighCallback)
GPIO.add_event_detect(self.__input1, GPIO.RISING, callback=self.Input1HighCallback)
self.__service = rospy.Service('gpio/output_cmd', gpio_output, self.OutputCommand)
self.__gpi_pub = rospy.Publisher('gpio/input_cmd', gpio_input, queue_size=3)
# Timers for button bounce
self.__input0_time = rospy.Time.now()
self.__input1_time = rospy.Time.now()
类的其余部分由回调函数组成。
第一个与ROS服务相关。当在gpio/output_cmd服务上收到对服务的调用时，将调用此回调。根据服务调用中包含的索引，它将GPIO输出设置为给定值并返回True。
def OutputCommand(self, request):
if(request.index == 0):
gpo = self.__output0
elif(request.index == 1):
gpo = self.__output1
GPIO.output(gpo, request.value)
return True
其余的回调函数彼此相似，当其中一个按钮被按下，导致相关GPIO输入引脚变为高电平时被调用。如果在过去200毫秒内没有按下按钮，则为gpio/input_cmd主题创建一条消息，其中包含GPIO的索引和True值，因为在我们的情况下，回调只会在输入引脚变为高电平时被调用。如果您的项目关心按钮释放时的情况，您可以为此操作添加一个回调，将消息中的值设置为false。然后将消息发布到主题。
def Input0HighCallback(self, channel):
# Check for button bounce
current_time = rospy.Time.now()
delta_time = (current_time - self.__input0_time).to_sec() # Floating point seconds
if delta_time > 0.200:
input_cmd = gpio_input()
input_cmd.index = 0
input_cmd.value = True
self.__gpi_pub.publish(input_cmd)
self.__input0_time = current_time
def Input1HighCallback(self, channel):
# Check for button bounce
current_time = rospy.Time.now()
delta_time = (current_time - self.__input1_time).to_sec() # Floating point seconds
if delta_time > 0.200:
input_cmd = gpio_input()
input_cmd.index = 1
input_cmd.value = True
self.__gpi_pub.publish(input_cmd)
self.__input1_time = current_time
以下是Rodney的更新电路图，其中包含了双色LED和按钮如何连接到树莓派GPIO。电路图的全尺寸图像可在绘图zip文件中获得。
请注意，自上一篇文章以来，我对电路进行了另一次更改。Teensy不再由树莓派供电。Teensy底部有一条轨迹，可以将其切断以将VIN与VUSB分离。
Rodney包的更新
rodney节点将负责设置LED的状态，并监控发布按钮状态变化的主题。我们还需要更新它，以便它可以为两个新任务提供任务数据。
第一个更改将是launch文件夹中的rodney.launch文件。此文件配置并启动构成系统的所有ROS节点。
我们需要将新的pi_io节点添加到启动文件中，以便在启动机器人代码时运行新代码。
<node pkg="pi_io" type="pi_io_node.py" name="pi_io_node"/>
为了让机器人能够导航到家中的各个位置，我们将为其提供一个存储在yaml文件中的航路点列表。我们将修改rodney节点，以便在启动时将文件名传递给它。为了帮助我们识别哪个航路点文件与哪个地图对应，航路点文件将与地图文件同名，并以_patrol作为后缀。例如，我的系统默认加载的地图名为second_floor，与此地图对应的航路点文件名为second_floor_patrol.yaml。我们将修改启动rodney节点的启动文件行，以便将文件名作为参数传递。文件名将紧随标志“-m”之后。
<node pkg="rodney" type="rodney_node" name="rodney"
args="-m $(find rodney)/maps/$(arg map_file)_patrol.yaml" output="screen">
<rosparam command="load" file="$(find rodney)/config/config.yaml"/>
</node>
正如您从启动文件片段中看到的，包含航路点的文件存储在map文件夹中。文件中的每个航路点都包含一个x和y位置以及机器人到达目标时应面向的方向。这个方向或姿态是一个四元数。在“使用代码”部分，我将展示一种方便的方法来计算地图上任何位置的位置和姿态值。
航点应从“w1”开始，您可以拥有任意数量的航点，只要它们是连续的。在搜索要传递消息的人时，机器人将按升序然后降序依次访问每个航点。文件中还应包含一个主航点，机器人将在接到“回家”指令时导航到该航点。
航路点文件示例
# Waypoints must start at w1 and be consecutive
# Also have an home location
w1:
position:
x: -0.328835725784
y: -0.529747009277
orientation:
x: 0.0
y: 0.0
z: 0.273852223218
w: 0.961771781577
w2:
position:
x: 1.31689155102
y: -0.944578707218
orientation:
x: 0.0
y: 0.0
z: 0.732759099845
w: 0.680488134793
w3:
position:
x: 3.66307258606
y: -0.040109038353
orientation:
x: 0.0
y: 0.0
z: 0.413215093458
w: 0.910633453448
w4:
position:
x: 6.55329608917
y: 1.04117441177
orientation:
x: 0.0
y: 0.0
z: 0.914737463209
w: -0.404048726519
home:
position:
x: 0.0451934337616
y: 0.0451934337616
orientation:
x: 0.0
y: 0.0
z: 0.224733375634
w: 0.974420294265
现在是rodney_node.cpp文件的更改。
main函数现在处理包含航路点yaml文件的参数解析。此文件名传递给rodney_node类的构造函数。
int main(int argc, char **argv)
{
ros::init(argc, argv, "rodney");
ros::NodeHandle nh;
ros::NodeHandle nh_private("~");
std::string waypoints_filename;
int opt;
/* argument -m gives the file containing the patrol pose positions */
while((opt = getopt(argc, argv, ":m:")) != -1)
{
switch(opt)
{
case 'm':
waypoints_filename = std::string(optarg);
break;
}
}
RodneyNode rodney_node(nh, nh_private, waypoints_filename);
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;
}
构造函数本身有一些小的更改。它获取航路点文件名并将其存储在成员变量waypoints_filename_中。
构造函数还订阅了来自GPIO节点的新主题gpio/input_cmd，注册了当主题上收到消息时要调用的回调函数。
// Raspberry Pi GPIO push buttons
gpio_sub_ = nh_.subscribe("gpio/input_cmd", 5, &RodneyNode::gpioCallBack, this);
构造函数中的最后一个更改是等待gpio/output_cmd服务可用，然后调用新函数missionNotRunning，该函数将GPIO输出设置为点亮LED的绿色部分。等待服务可用可以阻止我们在pi_io节点创建服务之前调用服务。
// Waiting for GPIO service to be available
ros::service::waitForService("gpio/output_cmd");
missionNotRunning();
missionNotRunning和missionRunning函数在我们切换任务运行状态时被调用。它们设置mission_running_成员变量的状态，并使用服务设置双色LED所需的亮度状态。
void RodneyNode::missionNotRunning(void)
{
mission_running_ = false;
// Red LED off, Green LED on
ros::ServiceClient client = nh_.serviceClient<pi_io::gpio_output>("gpio/output_cmd");
pi_io::gpio_output srv;
srv.request.index = 0;
srv.request.value = true;
client.call(srv);
srv.request.index = 1;
srv.request.value = false;
client.call(srv);
}
//---------------------------------------------------------------------------
void RodneyNode::missionRunning(void)
{
mission_running_ = true;
// Red LED on, Green LED off
ros::ServiceClient client = nh_.serviceClient<pi_io::gpio_output>("gpio/output_cmd");
pi_io::gpio_output srv;
srv.request.index = 0;
srv.request.value = false;
client.call(srv);
srv.request.index = 1;
srv.request.value = true;
client.call(srv);
}
当在gpio/input_cmd主题上收到消息时，将调用gpioCallBack函数。它检查哪个GPIO信号变为高电平，从而确定哪个按钮被按下。
如果按下了黑色按钮且任务正在运行，它将发布消息以取消任务。如果任务未运行，它将发布消息请求头部移动到用户输入位置。
如果按下黄色按钮且任务正在运行，它将发布消息以确认任务状态。如果任务未运行，它将发布消息请求执行任务4（回家任务）。消息中包含航路点文件名。
// callback for change in GPIO input
void RodneyNode::gpioCallBack(const pi_io::gpio_input::ConstPtr& msg)
{
switch(msg->index)
{
case 0:
// Black push button changed state
// If mission running and button pressed, send cancel mission message
if((mission_running_ == true) && (msg->value == true))
{
// Cancel the ongoing mission
std_msgs::Empty empty_msg;
cancel_pub_.publish(empty_msg);
}
// If mission not running and button pressed, move head to user input position
else if((mission_running_ == false) && (msg->value == true))
{
std_msgs::String mission_msg;
mission_msg.data = "J3^i^-";
mission_pub_.publish(mission_msg);
}
break;
case 1:
// Yellow push button
// If mission running and button pressed, send acknowledge mission stage message
if((mission_running_ == true) && (msg->value == true))
{
// Acknowledge a mission step
std_msgs::Empty empty_msg;
ack_pub_.publish(empty_msg);
}
// If mission not running and button pressed, send mission 4 command
else if((mission_running_ == false) && (msg->value == true))
{
std_msgs::String mission_msg;
mission_msg.data = "M4^"+ waypoints_filename_;
mission_pub_.publish(mission_msg);
missionRunning();
manual_locomotion_mode_ = false;
}
break;
default:
break;
}
last_interaction_time_ = ros::Time::now();
}
keyboardCallBack函数有一个变化。我们目前在测试机器人时使用远程机器上的键盘来指示应该执行任务。如果按下键“1”，我们为任务1形成一个任务请求消息并发布该消息。对于此任务，任务数据包括航路点文件名、要传递消息的人的ID以及要传递的消息文本。如果按下键“4”，我们创建一个任务4请求，类似于按下黄色按钮且没有任务运行时的请求。
// Start a mission
std_msgs::String mission_msg;
mission_msg.data = "M" + std::to_string(msg->code-keyboard::Key::KEY_0);
// Add on parameters for different missions
switch(msg->code)
{
case keyboard::Key::KEY_1:
// Mission 1 "Take a message to.."
// "M1^patrol poses file|id of person to search for|message to deliver"
mission_msg.data += "^" + waypoints_filename_ + "|1|Please walk Bonnie";
break;
case keyboard::Key::KEY_4:
// Mission 4 "Go Home"
// "M4^patrol poses file""
mission_msg.data += "^" + waypoints_filename_;
break;
default:
break;
}
mission_pub_.publish(mission_msg);
missionRunning();
启动文件还有一个我不情愿做出的改动。我偶尔注意到，当树莓派繁忙时，例如在计算导航路线时，LIDAR代码会停止生成带有激光扫描数据的消息。一旦进入这种状态，它就永远无法恢复，并且会不断返回超时。尽管代码来自第三方，但我们确实有源代码，但我急于完成项目的这一部分，目前不想花时间研究别人的代码。我通过fork GitHub仓库并修改文件node.cpp对代码进行了简单更改。更改的结果是在日志中引发ROS错误消息并调用ros::shutdown()来终止节点。更改后的代码可在源代码zip文件中找到。
为了使节点在此错误条件下重新启动，我在rodney.launch文件中的rplidar_ros标签中添加了respawn="true"。
<!-- The RPLidar and laser filter node
Have created symbolic link for /dev/ttyUSBn to be rplidar -->
<node pkg="rplidar_ros" type="rplidarNode" name="rplidar_node" respawn="true" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser"/>
<remap from="scan" to="scan_filter_input"/>
</node>
这不好，有点像个权宜之计，但自从添加它以来，机器人总是在偶尔发生的情况下恢复。可能是我们对树莓派要求太高了。Rodney一直都是一个原型，我的下一个机器人项目可能需要考虑更强大的单板计算机。
rodney_missions包的更新
我们最初设计rodney_missions节点是为了方便添加新任务。我们将对rodney_missions_node.py文件进行一些小改动，以适应这些新任务，并添加新文件take_message_to.py和go_home.py，其中将包含低级状态机。
让我们来看看这两个新的状态机。
任务1 - “给…带个口信”
这个任务是所有文章一直在努力实现的目标。这个状态机的所有代码都在take_message_to.py文件中。
我们通过PREPARE_MISSION状态进入这个低级状态机。传递给这个状态的任务数据包含航路点文件名、要搜索的人的ID和要传递的消息。这些参数由'|'字符分隔，所以这个状态做的第一件事就是将数据分成三个部分。然后它加载航路点文件，由于我们将自主导航，它确保LIDAR已启用。如果它成功打开文件并读取了航路点，那么它将转换到PREPARE_FOR_HEAD_MOVE状态，否则如果发生错误，它将中止任务并转换回高级状态机，我们称之为根状态机。
PREPARE_MISSION状态的代码保存在PrepareMission1类中。
# PREPARE_MISSION State. Prepares the mission by loading the waypoint file
class PrepareMission1(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['ready','error'],
input_keys=['mission_data'],
output_keys=['person_id','message','waypoints',
'current_waypoint','waypoint_direction','mission_status','start'])
self.__helper_obj = helper_obj
def execute(self, userdata):
# Parse the mission data using '|' as the delimiter
# parameters[0] will contain the filename of the file containing the poses
# parameters[1] will contain the id of the person to search for
# parameters[2] will contain the message to deliver
parameters = userdata.mission_data.split("|")
userdata.person_id = parameters[1]
userdata.message = parameters[2]
# Load patrol poses from the given patrol file
try:
with open(parameters[0], 'r') as stream:
try:
userdata.waypoints = yaml.load(stream)
userdata.current_waypoint = 0
userdata.waypoint_direction = True
userdata.mission_status = 'void' # This will be overwritten
next_outcome = 'ready'
except:
rospy.logerr('Bad waypoint file')
userdata.mission_status = 'Bad waypoint file'
next_outcome = 'error'
except:
rospy.logerr("Can't open waypoint file")
userdata.mission_status = "Can't open waypoint file"
next_outcome = 'error'
# Ensure the Lidar is enabled
self.__helper_obj.LidarEnable()
# Indicate that when we start scanning for faces that it is a new scan
userdata.start = True
return next_outcome
PREPARE_FOR_HEAD_MOVE状态负责计算下一个头部位置，因为头部/摄像头会移动以扫描机器人的视野。如果仍有头部位置需要扫描，它将转换到MOVE_HEAD状态，否则将转换到DEFAULT_HEAD_POSITION状态。
PREPARE_FOR_HEAD_MOVE状态的代码保存在PrepareToMoveHead类中。
# PREPARE_FOR_HEAD_MOVE State. Prepares for the next head position
class PrepareToMoveHead(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['scan_complete','move_head'],
input_keys=['start_in'],
output_keys=['start_out','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 head scan
if userdata.start_in == True:
userdata.start_out = False
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 scan 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 = 'scan_complete'
else:
next_outcome = 'move_head'
return next_outcome
MOVE_HEAD状态是一种特殊类型的状态，称为SimpleActionState。我们在文章的第4部分中介绍了这些类型的状态。执行的动作是将头部移动到由PREPARE_FOR_HEAD_MOVE状态设置的位置。动作完成后，状态机将转换到SCAN_FOR FACES状态。该动作也可以被取消或中止，这将导致转换回根状态机。
SCAN_FOR_FACES状态又是一个SimpleActionState。这次，调用的动作是人脸识别包的一部分，它将检查相机中的下一张图像以识别已知对象。动作完成后，状态机将转换到CHECK_FOR_SUBJECT状态。该动作也可以取消或中止，这将导致转换回根状态机。
CHECK_FOR_SUBJECT状态检查前一状态的结果，以查看机器人正在搜索的人的ID是否与结果中的任何人匹配。如果找到了目标，则通过创建并发布ROS主题到语音和动画头部节点来传递消息。然后状态机将转换到WAIT_FOR_USER状态。如果未找到目标，则将转换回PREPARE_FOR_HEAD_MOVE。
CHECK_FOR_SUBJECT状态的代码保存在CheckForSubject类中。
# CHECK_FOR_SUBJECT state. Checks the results from the previous state to see
# if person searching for was found
class CheckForSubject(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['seen','not_seen'],
input_keys=['person_id','message','seen_ids','seen_names'])
self.__helper_obj = helper_obj
def execute(self, userdata):
next_outcome = 'not_seen'
# Was anyone see?
if len(userdata.seen_ids) > 0:
# at least one person was seen
for idx, val in enumerate(userdata.seen_ids):
if str(val) == userdata.person_id:
# We have found who we were looking for
next_outcome = 'seen'
rospy.loginfo("I have found %s, delivering message",
userdata.seen_names[idx])
greeting = 'Hello ' + userdata.seen_names[idx] +
' I have a message for you'
# Speak greeting
self.__helper_obj.Speak(greeting, greeting)
rospy.sleep(2.0)
# Speak message
self.__helper_obj.Speak(userdata.message, userdata.message)
return next_outcome
WAIT_FOR_USER状态在此状态中等待，直到收到确认消息，表明用户确认收到口头消息，或者直到60秒过去。如果收到确认，则状态机将转换到根状态机。如果60秒过去，则状态机将转换回SCAN_FOR_FACES状态。该状态也可以被抢占，这将导致转换回根状态机。
WAIT_FOR_USER状态的代码保存在WaitForMsg类中。
# WAIT_FOR_USER state. Waits for the ack message for a set time
class WaitForMsg(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['ack','timeout','preempted'],
output_keys=['mission_status'])
self.__helper_obj = helper_obj
self.__mutex = threading.Lock()
self.__msg = None
self.__subscriber = rospy.Subscriber('/missions/acknowledge',
Empty, self.callBack, queue_size=1)
def callBack(self, msg):
# Indicate a message was received
self.__mutex.acquire()
self.__msg = msg
self.__mutex.release()
def execute(self, userdata):
# Clear any previous ack message
self.__mutex.acquire()
self.__msg = None
self.__mutex.release()
# Give the user 60 seconds to acknowledge the message
timeout = rospy.Time.now() + rospy.Duration.from_sec(60.0)
message_arrived = False
preempted = False
while rospy.Time.now() < timeout and message_arrived == False and preempted == False:
# Check to see if message arrived
self.__mutex.acquire()
if self.__msg is not None:
# Message has arrived
message_arrived = True
self.__mutex.release()
# Check mission was cancelled
if self.preempt_requested():
self.service.preempt()
preempted = True
# Why did the loop terminate
if preempted == True:
# Report with voice that the mission was cancelled
message = 'Mission one cancelled'
self.__helper_obj.Speak(message, message)
# set the mission status
userdata.mission_status = 'Take a message mission cancelled'
next_outcome = 'preempted'
elif message_arrived == True:
userdata.mission_status = 'Take a message mission complete'
next_outcome = 'ack'
else:
next_outcome = 'timeout'
return next_outcome
DEFAULT_HEAD_POSITION状态又是一个SimpleActionState。它将调用一个动作将头部/摄像头移动到默认位置。当机器人移动时，我们希望头部向前。一旦头部移动完成，它将转换到PREPARE_TO_MOVE状态。该动作也可以被取消或中止，这将导致转换回根状态机。
PREPARE_TO_MOVE状态负责从航路点列表中提取下一个航路点。如果它到达列表末尾，它将开始沿相反方向遍历列表。根据航路点数据，它构建下一个状态MOVE的导航目标。
PREPARE_TO_MOVE状态的代码保存在PrepareToMove类中。
# PREPARE_TO_MOVE State. Reads the next waypoint to move the base to
class PrepareToMove(State):
def __init__(self):
State.__init__(self, outcomes=['done'],
input_keys=['waypoints','current_waypoint_in','waypoint_direction_in'],
output_keys=['current_waypoint_out',
'destination','waypoint_direction_out','start'])
def execute(self, userdata):
# Extract the next waypoint but which way along the waypoints are we travelling
if userdata.waypoint_direction_in == True:
# Incrementing along the waypoints
next_waypoint = userdata.current_waypoint_in + 1
waypoint_id = 'w' + str(next_waypoint)
# If next waypoint does not exist (reached end of list),
# we need to work backwards along the list
if not waypoint_id in userdata.waypoints:
next_waypoint = userdata.current_waypoint_in - 1
userdata.waypoint_direction_out = False
# Allow for only one waypoint
if next_waypoint == 0:
next_waypoint = 1
# next waypoint updated so update the waypoint_id string
waypoint_id = 'w' + str(next_waypoint)
else:
# Decrementing along the waypoints
next_waypoint = userdata.current_waypoint_in - 1
# If next point is now zero, we have reached the start of list and
# we need to work forwards along the list
if next_waypoint == 0:
next_waypoint = 1
userdata.waypoint_direction_out = True
waypoint_id = 'w' + str(next_waypoint)
# Copy the waypoint data in to a PoseStamped message and userdata
waypoint = userdata.waypoints[waypoint_id]
target_pose = PoseStamped()
target_pose.header.frame_id = 'map'
target_pose.pose.position.x = waypoint['position']['x']
target_pose.pose.position.y = waypoint['position']['y']
target_pose.pose.position.z = 0.0
target_pose.pose.orientation.x = waypoint['orientation']['x']
target_pose.pose.orientation.y = waypoint['orientation']['y']
target_pose.pose.orientation.z = waypoint['orientation']['z']
target_pose.pose.orientation.w = waypoint['orientation']['w']
userdata.destination = target_pose
userdata.current_waypoint_out = next_waypoint
# Once we reach the new destination we will start
# a new head scan so reset flag here
userdata.start = True
return 'done'
MOVE状态又是一个SimpleActionState。这次，执行的动作是导航到由航路点数据设置的姿态。动作完成后，状态机将转换到PREPARE_FOR_HEAD_MOVE状态。该动作也可以被取消或中止，这将导致转换回根状态机。
此状态机的其余代码包含在Mission1StateMachine类中。它从前面描述的类构建状态机。还有许多函数在Simple Action States完成时被调用。这些函数主要关注报告动作是否被取消或中止。
# Child (derived) class. Parent class is StateMachine
class Mission1StateMachine(StateMachine):
def __init__(self, helper_obj):
StateMachine.__init__(self, outcomes=['complete','preempted','aborted'],
input_keys=['mission_data'], output_keys=['mission_status'])
self.__helper_obj = helper_obj
with self:
# This state will prepare for the mission
StateMachine.add('PREPARE_MISSION',
PrepareMission1(self.__helper_obj),
transitions={'ready':'PREPARE_FOR_HEAD_MOVE','error':'aborted'})
# Set up action goal for deafult head position
default_position_pan, default_position_tilt = self.__helper_obj.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,
goal = head_goal,
result_cb = self.defaultHeadPositionComplete,
output_keys=['mission_status']),
transitions={'succeeded':'PREPARE_TO_MOVE',
'preempted':'preempted','aborted':'aborted'})
# The next state prepares for each nav goal request
StateMachine.add('PREPARE_TO_MOVE',
PrepareToMove(),
transitions={'done':'MOVE'},
remapping={'current_waypoint_in':'current_waypoint',
'current_waypoint_out':'current_waypoint',
'waypoint_direction_in':'waypoint_direction',
'waypoint_direction_out':'waypoint_direction'})
# This state uses an Action to move the robot to the required goal
StateMachine.add('MOVE',
SimpleActionState('move_base',
MoveBaseAction,
goal_slots=['target_pose'],
result_cb = self.moveComplete,
output_keys=['mission_status']),
transitions={'succeeded':'PREPARE_FOR_HEAD_MOVE',
'preempted':'preempted', 'aborted':'aborted'},
remapping={'target_pose':'destination'})
# This state will calculate the next head/camera position
StateMachine.add('PREPARE_FOR_HEAD_MOVE',
PrepareToMoveHead(self.__helper_obj),
transitions={'scan_complete':'DEFAULT_HEAD_POSITION',
'move_head':'MOVE_HEAD'},
remapping={'start_in':'start','start_out':'start'})
# This state will call the action to move the head/camera
StateMachine.add('MOVE_HEAD',
SimpleActionState('head_control_node',
point_headAction,
goal_slots=['absolute','pan','tilt'],
result_cb = self.moveHeadComplete,
output_keys=['mission_status']),
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_slots=['ids_detected','names_detected'],
result_cb = self.scanForFaceComplete,
output_keys=['mission_status']),
transitions={'succeeded':'CHECK_FOR_SUBJECT',
'preempted':'preempted','aborted':'aborted'},
remapping={'ids_detected':'seen_ids',
'names_detected':'seen_names'})
# This state will check the results of the scan and decide on the next action
StateMachine.add('CHECK_FOR_SUBJECT',
CheckForSubject(self.__helper_obj),
transitions={'seen':'WAIT_FOR_USER',
'not_seen':'PREPARE_FOR_HEAD_MOVE'})
# This state will wait for the acknowledge message or will timeout
# if the message is not received in a set time
StateMachine.add('WAIT_FOR_USER',
WaitForMsg(self.__helper_obj),
transitions={'ack':'complete','timeout':'SCAN_FOR_FACES',
'preempted':'preempted'})
def defaultHeadPositionComplete(self, userdata, status, result):
self.__helper_obj.CameraAtDefaultPos(userdata, status, result)
if status == GoalStatus.PREEMPTED:
# Report with voice that the mission was cancelled
message = 'Mission number 1 cancelled'
self.__helper_obj.Speak(message, message)
# set the mission status
userdata.mission_status = 'Take a message mission cancelled'
elif status == GoalStatus.ABORTED:
# Report with voice that the mission was aborted
message = 'Warning mission number 1 aborted'
self.__helper_obj.Speak(message, message + ':(')
# set the mission status
userdata.mission_status = 'Take a message mission failed during move head action'
def moveComplete(self, userdata, status, result):
if status == GoalStatus.PREEMPTED:
# Report with voice that the mission was cancelled
message = 'Mission number 1 cancelled'
self.__helper_obj.Speak(message, message)
# set the mission status
userdata.mission_status = 'Take a message mission cancelled'
elif status == GoalStatus.ABORTED:
# Report with voice that the mission was aborted
message = 'Warning mission number 1 aborted'
self.__helper_obj.Speak(message, message + ':(')
# set the mission status
userdata.mission_status = 'Take a message mission failed during base move action'
def moveHeadComplete(self, userdata, status, result):
if status == GoalStatus.PREEMPTED:
# Report with voice that the mission was cancelled
message = 'Mission number 1 cancelled'
self.__helper_obj.Speak(message, message)
# set the mission status
userdata.mission_status = 'Take a message mission cancelled'
elif status == GoalStatus.ABORTED:
# Report with voice that the mission was aborted
message = 'Warning mission one aborted'
self.__helper_obj.Speak(message, message + ':(')
# set the mission status
userdata.mission_status = 'Take a message mission failed during head move'
def scanForFaceComplete(self, userdata, status, result):
if status == GoalStatus.PREEMPTED:
# Report with voice that the mission was cancelled
message = 'Mission number 1 cancelled'
self.__helper_obj.Speak(message, message)
# set the mission status
userdata.mission_status = 'Take a message mission cancelled'
elif status == GoalStatus.ABORTED:
# Report with voice that the mission was aborted
message = 'Warning mission one aborted'
self.__helper_obj.Speak(message, message + ':(')
# set the mission status
userdata.mission_status = 'Take a message mission failed during face scan'
有关ROS状态机的更多信息，请参阅文章的第4部分和smach ROS包的文档。
任务4 - “回家”
这个状态机的所有代码都在go_home.py文件中。
我们通过PREPARE_MISSION状态进入这个低级状态机。传递给这个状态的任务数据包含航路点文件名。它加载航路点文件，为家庭航路点构建导航目标，并且由于我们将自主导航，它确保LIDAR已启用。如果它读取了文件并在文件中找到了家庭航路点，那么它将转换到DEFAULT_HEAD_POSITION状态，否则如果发生错误，它将转换回根状态机。
PREPARE_MISSION状态的代码保存在PrepareMission4类中。
# PREPARE_MISSION State. Prepares the mission by loading the waypoint file
class PrepareMission4(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['ready','error'],
input_keys=['mission_data'],
output_keys=['destination','mission_status'])
self.__helper_obj = helper_obj
def execute(self, userdata):
# mission_data will contain the filename of the file
# containing the waypoints including the home waypoint
# Ensure the Lidar is enabled
self.__helper_obj.LidarEnable()
# Load the waypoints
try:
with open(userdata.mission_data, 'r') as stream:
try:
waypoints = yaml.load(stream)
next_outcome = 'ready'
except:
rospy.logerr('Bad waypoint file')
userdata.mission_status = 'Bad waypoint file'
next_outcome = 'error'
except:
rospy.logerr("Can't open waypoint file")
userdata.mission_status = "Can't open waypoint file"
next_outcome = 'error'
if next_outcome == 'ready':
# Check the home waypoint exists
if 'home' in waypoints:
# Copy the home waypoint data in to a PoseStamped message and userdata
waypoint = waypoints['home']
target_pose = PoseStamped()
target_pose.header.frame_id = 'map'
target_pose.pose.position.x = waypoint['position']['x']
target_pose.pose.position.y = waypoint['position']['y']
target_pose.pose.position.z = 0.0
target_pose.pose.orientation.x = waypoint['orientation']['x']
target_pose.pose.orientation.y = waypoint['orientation']['y']
target_pose.pose.orientation.z = waypoint['orientation']['z']
target_pose.pose.orientation.w = waypoint['orientation']['w']
userdata.destination = target_pose
else:
rospy.logerr('No home waypoint in file')
userdata.mission_status = 'No home waypoint in file'
next_outcome = 'error'
return next_outcome
DEFAULT_HEAD_POSITION状态是一个SimpleActionState。它将调用一个动作将头部/摄像头移动到默认位置。同样，当机器人移动时，我们希望头部向前。一旦头部移动完成，它将转换到MOVE状态。该动作也可以被取消或中止，这将导致转换回根状态机。
MOVE状态又是一个SimpleActionState。这次，执行的动作是导航到由航路点数据设置的家庭姿态。动作完成后，状态机将转换回根状态机。该动作也可以被取消或中止，这将导致转换回根状态机。
此状态机的其余代码包含在Mission4StateMachine类中。它从前面描述的类构建状态机。还有许多函数在Simple Action States完成时被调用。这些函数主要关注报告动作是否被取消或中止。
# Child (derived) class. Parent class is StateMachine
class Mission4StateMachine(StateMachine):
def __init__(self, helper_obj):
StateMachine.__init__(self, outcomes=['complete','preempted','aborted'],
input_keys=['mission_data'], output_keys=['mission_status'])
self.__helper_obj = helper_obj
with self:
# This state will prepare for the mission
StateMachine.add('PREPARE_MISSION',
PrepareMission4(self.__helper_obj),
transitions={'ready':'DEFAULT_HEAD_POSITION','error':'complete'})
# Set up action goal for deafult head position
default_position_pan, default_position_tilt = self.__helper_obj.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,
goal = head_goal,
result_cb = self.defaultHeadPositionComplete,
output_keys=['mission_status']),
transitions={'succeeded':'MOVE',
'preempted':'preempted','aborted':'aborted'})
# This state uses an Action to move the robot to the required goal
StateMachine.add('MOVE',
SimpleActionState('move_base',
MoveBaseAction,
goal_slots=['target_pose'],
result_cb = self.moveComplete,
output_keys=['mission_status']),
transitions={'succeeded':'complete',
'preempted':'preempted', 'aborted':'aborted'},
remapping={'target_pose':'destination'})
def defaultHeadPositionComplete(self, userdata, status, result):
self.__helper_obj.CameraAtDefaultPos(userdata, status, result)
if status == GoalStatus.PREEMPTED:
# Report with voice that the mission was cancelled
message = 'Mission number 4 cancelled'
self.__helper_obj.Speak(message, message)
# set the mission status
userdata.mission_status = 'Go home mission cancelled'
elif status == GoalStatus.ABORTED:
# Report with voice that the mission was aborted
message = 'Warning mission 4 aborted'
self.__helper_obj.Speak(message, message + ':(')
# set the mission status
userdata.mission_status = 'Go home mission failed during move head action'
def moveComplete(self, userdata, status, result):
if status == GoalStatus.PREEMPTED:
# Report with voice that the mission was cancelled
message = 'Mission number 4 cancelled'
self.__helper_obj.Speak(message, message)
# set the mission status
userdata.mission_status = 'Go home mission cancelled'
elif status == GoalStatus.ABORTED:
# Report with voice that the mission was aborted
message = 'Warning mission 4 aborted'
self.__helper_obj.Speak(message, message + ':(')
# set the mission status
userdata.mission_status = 'Go home mission failed during base move action'
elif status == GoalStatus.SUCCEEDED:
userdata.mission_status = 'Go home mission complete'
根状态机的更改
剩下的就是对rodney_missions_node.py文件进行一些更改。它包含根状态机，现在需要将每个低级状态机作为状态包含在内。
我们已经有了返回默认头部位置的代码，但由于我们还添加了将头部设置为用户输入角度的功能，我们需要定义这个位置。这意味着我们需要对rodney_missions_node.py文件进行一些小改动以实现此功能。
MissionsHelper类的构造函数现在包含从参数服务器读取用于用户输入位置的角度的代码。请记住，ROS使用弧度表示角度值。
self.__user_input_position_pan = rospy.get_param("~head/user_position/pan", 0.0)
self.__user_input_position_tilt = rospy.get_param("~head/user_position/tilt", -0.5)
该类还包含一个返回这些值的新函数。
def UserInputPosition(self):
return self.__user_input_position_pan, self.__user_input_position_tilt
根状态机的PREPARE状态现在需要包含代码，不仅要在请求时设置用户输入角度，还要在请求时转换到新的低级状态机。PREPARE状态的完整代码现在看起来像这样。
# The PREPARE state
class Prepare(State):
def __init__(self, helper_obj):
State.__init__(self, outcomes=['mission1','mission2',
'mission4','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 separated by '^'
retVal = 'done_task';
# Split into parameters using '^' as the delimiter
parameters = userdata.mission.split("^")
if parameters[0] == 'M1':
# Mission 1 to search for a known person and deliver a message
# parameter[1] contains filename of file containing poses for the patrol,
# the id of the person to search for and the message to deliver
userdata.mission_data = parameters[1]
retVal = 'mission1'
elif 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] == 'M4':
# Mission 4 is go home. parameter[1] contains filename of file containing poses
userdata.mission_data = parameters[1]
retVal = 'mission4'
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', 'i' or '-'
# parameters[2] will either be 'l', 'r' or '-'
# Check for command
if 'c' in parameters[1]:
# Move to default position
retVal = 'head_default'
elif 'i' in parameters[1]:
# Move to user input position. This position is a good position
# for user input at the screen
pan_position, tilt_position = self.__helper_obj.UserInputPosition()
userdata.user_data_absolute = True # This will be a actual position move
userdata.user_data_pan = pan_position
userdata.user_data_tilt = tilt_position
retVal = 'move_head'
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'
elif parameters[0] == 'J4':
# Simple job to toggle the LIDAR on/off
self.__helper_obj.ToggleLidar()
return retVal
其余的更改是RodneyMissionNode类，只需将新的低级状态机添加到根状态机。
这是将新状态机添加到根状态机的示例
# ------------------------- Sub State machine for mission 1 ---------------------
# Create a sub state machine for mission 1 - take a message to
self.__sm_mission1 = missions_lib.Mission1StateMachine(self.__missions_helper)
# Now add the sub state machine for mission 1 to the top level one
StateMachine.add('MISSION1',
self.__sm_mission1,
transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'})
当收到取消任务的消息时，我们还必须调用抢占正在运行的低级状态机。CancelCallback函数现在看起来像这样
# 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_mission1.is_running():
self.__sm_mission1.request_preempt()
elif self.__sm_mission2.is_running():
self.__sm_mission2.request_preempt()
elif self.__sm_mission4.is_running():
self.__sm_mission4.request_preempt()
机器人硬件
当前电路图的全尺寸图像可在图表 zip 文件夹中找到，以及显示所有节点和主题的 rqt_graph 图像的全尺寸副本。
截至目前，项目的完整物料清单可在此处获取。
在文章的第1部分中，我提到了我在树莓派上使用的Ubiquity机器人镜像。有关如何安装镜像、安装额外软件以及为项目配置它的说明可在此处获取。
Using the Code
像往常一样，我将在机器人硬件上运行代码，并在Linux PC上运行测试工具和手动控制节点。在下面的细节中，我将这台PC称为工作站。
在Pi上构建ROS包（机器人硬件）
如果尚未完成，请在树莓派上创建catkin工作区并使用以下命令进行初始化
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
将face_recognition、face_recognition_msgs、head_control、imu_calib、pan_tilt、pi_io、rodney、rodney_missions、ros-keyboard、rplidar-ros、servo_msgs、speech、tacho_msgs和thunderborg等包复制到~/rodney_ws/src文件夹中。
使用以下命令构建代码
$ cd ~/rodney_ws/
$ catkin_make
检查构建是否无任何错误地完成。
您还需要编译并将草图下载到 Teensy 3.5。
在工作站上构建ROS包
在工作站上，我们希望运行键盘、操纵杆和心跳节点，以便能够远程控制实际的机器人硬件。
使用以下命令创建一个工作空间
$ mkdir -p ~/test_ws/src
$ cd ~/test_ws/
$ catkin_make
将rodney、joystick和ros-keyboard包复制到~/test_ws/src文件夹，然后使用以下命令构建代码
$ cd ~/test_ws/
$ catkin_make
检查构建是否无任何错误地完成。
技巧
在工作站和树莓派上运行ROS代码和工具时，可能需要在多个终端中重复输入大量命令。在接下来的部分中，我将提供完整的输入命令，但这里有一些可以为您省去所有输入的技巧。
在 Raspberry Pi 上，为了节省输入 "source devel/setup.bash"，我已将其添加到 Raspberry Pi 的 .bashrc 文件中。
$ cd ~/
$ nano .bashrc
然后将 "source /home/ubuntu/rodney_ws/devel/setup.bash" 添加到文件末尾，保存并退出。
在工作站上运行测试代码和工具时，它也需要知道 ROS master 的位置，所以我已经将以下内容添加到工作站的 .bashrc 文件中。
alias rodney='source ~/test_ws/devel/setup.bash; \
export ROS_MASTER_URI=http://ubiquityrobot:11311'
然后，只需在终端中输入“rodney”，两个命令就会运行，省去了大量的输入。
您还可以节省一些输入，因为某些ROS工具支持TAB补全。例如，键入rosrun rosserial_，然后按Tab键自动补全rosrun rosserial_python。
航路点
你对四元数了解多少？你是否乐意设置每个航路点的方向值，使其与你希望机器人到达航路点时面对的方向相匹配？如果你对这些问题回答“很棒”和“是”，那么请继续创建你的航路点文件。另一方面，如果你像我一样需要一些帮助，那么请继续阅读下面的内容。
我们将启动机器人代码和rviz，并使用rviz中的导航工具设置导航目标，就像我们在此系列文章的上一篇中所做的那样。
在机器人硬件上，运行以下命令以启动系统中所有当前节点。我将使用我的默认地图，但您可以通过在roslaunch命令末尾添加“map_file:=my_map_filename”来设置地图。
$ source rodney_ws/devel/setup.bash
$ roslaunch rodney rodney.launch
在工作站上，运行以下命令启动远程控制节点
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ roslaunch rodney remote.launch
一个标题为“ROS 键盘输入”的小窗口应该正在运行。输入键盘按键时，请确保小窗口处于焦点。
将此窗口置于焦点并按键盘上的“m”键，将机器人设置为手动模式，使其在以下步骤中不会移动。
在工作站上，在另一个终端中运行以下命令以启动rviz
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ roslaunch rodney rviz.launch
在工作站上，在另一个终端中运行以下命令
$ rostopic echo /move_base/goal
现在通过点击“2D Nav Goal”按钮选择一个航路点位置，然后像设置目标一样在地图上点击/拖动大绿色箭头。
目标值将由rostopic echo终端显示。将这些值复制到您的航路点文件中。对您要添加到文件中的每个航路点重复此操作。不要忘记在文件中设置一个家庭航路点。
关闭机器人和工作站代码。
测试
测试设置
在机器人硬件上，运行以下命令以启动系统中所有当前节点。我将使用我的默认地图，但您可以通过在roslaunch命令末尾添加“map_file:=my_map_filename”来设置地图。这次，您的航路点文件也应该被加载。
$ source rodney_ws/devel/setup.bash
$ roslaunch rodney rodney.launch
在工作站上，运行以下命令启动远程控制节点
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ roslaunch rodney remote.launch
一个标题为“ROS 键盘输入”的小窗口应该正在运行。输入键盘按键时，请确保小窗口处于焦点。
在工作站上，在另一个终端中运行以下命令以启动rviz
$ source test_ws/devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ roslaunch rodney rviz.launch
用户输入位置
检查双色LED是否亮绿色，因为机器人没有执行任务。
按下黑色按钮，头部应移动到用户输入位置。
定位机器人
在我们让机器人自由活动之前，我们仍然需要像上一篇文章中那样手动定位机器人。我希望将来能添加一些自定位功能，但目前，请配置rviz以显示机器人模型或base_link轴、激光扫描、地图和姿态估计。还要确保地图是固定帧。
从显示中我们可以看到激光扫描与地图不匹配，姿态估计散布在周围。因此，在给机器人设定导航目标之前，我们需要改善其定位。
我们将执行的第一个操作是使用rviz为机器人提供改进的定位。点击“2D姿态估计”按钮，估计机器人的真实位置和姿态，然后在地图上点击/拖动大绿色箭头以设置初始姿态。您可以一直这样做，直到激光扫描与地图匹配。
我们现在有了一个良好的初始姿态，但姿态估计仍然不准确。我们可以通过在手动模式下驾驶机器人来改善这些。原地旋转是一个很好的操作。在移动机器人时，您应该会看到姿态估计在rviz中收敛到机器人的位置。
回家
根据前面的步骤，机器人仍将处于手动模式，因此请遥控机器人，使其处于与家庭航路点不同的位置。
现在按下黄色按钮。双色LED亮红色，机器人应自动导航到家庭位置。
取消任务
将机器人置于手动模式，并将其从家中的位置遥控离开。再次按下黄色按钮，但这次在机器人到达家中的位置之前，按下黑色按钮取消任务。机器人应该停止移动，双色LED应该亮绿色。
传递消息给
好了，我们现在即将测试我们为机器人编写的每一行代码！
rodney包中用于此任务的测试代码将导致机器人搜索人脸识别主题中ID为“1”的人。这在这些文章的第2部分中设置。传递的消息还将要求该人遛狗。我想您可能已经发现了这一点，如果您正在构建自己的机器人，现在可能已经更改了ID和/或消息。
将ROS 键盘输入窗口置于焦点，然后按下“1”键（不是数字键盘上的）。机器人应该依次访问每个航路点，并在每个航路点移动头部，搜索ID为“1”的人。
找到主题后，将发送消息。消息发送60秒后，如果未采取任何操作，机器人将通过首先检查主题是否仍在当前相机图像中来恢复搜索。消息第二次发送后，按下黄色按钮确认消息。
关注点
我们现在几乎完成了这个机器人项目的初步构想。在接下来的部分（我预计也是最后一部分）中，我将研究
- 从网页浏览器分配任务和任务数据，这将完成设计目标6
- 通电后运行任务2，即问候任务
- 看看我们能否让机器人进行自定位操作
虽然我在源代码zip文件中包含了正在开发中的所有代码的当前版本，但代码也可在我的GitHub网站上获取。每个包都有自己的仓库。请随意为您的项目分叉仓库，进行更新，如果您觉得任何更改对我有用或对他人有益，请创建拉取请求。
历史
- 2019/05/03：首次发布