通过传感器 和 Navigation stack 可以搭建好初始化的导航避障系统，在RVIZ中使用2D navigation goal 可以实现任务目标点和位姿的发布。
查看话题和消息格式
发布的话题为/move_base_simple/goal
使用
rostopic echo /move_base_simple/goal
打印发布的任务消息
这里简单复制一份过来，一起看下
header:
seq: 0
stamp:
secs: 425
nsecs: 821000000
frame_id: "map"
pose:
position:
x: 4.49436187744
y: -4.96984291077
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.698584081705
w: 0.715527973449
---
消息格式可以看出来是：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
geometry_msgs/Quaternion orientation
这里值得注意看下 frame_id
四元数介绍
顺便介绍下欧拉角到四元数的变换：
给定一个欧拉旋转(X, Y, Z)（即分别绕x轴、y轴和z轴旋转X、Y、Z度），则对应的四元数为：
x = sin(Y/2)sin(Z/2)cos(X/2)+cos(Y/2)cos(Z/2)sin(X/2)
y = sin(Y/2)cos(Z/2)cos(X/2)+cos(Y/2)sin(Z/2)sin(X/2)
z = cos(Y/2)sin(Z/2)cos(X/2)-sin(Y/2)cos(Z/2)sin(X/2)
w = cos(Y/2)cos(Z/2)cos(X/2)-sin(Y/2)sin(Z/2)sin(X/2)
q = ((x, y, z), w)
建立功能包
建立ros 包
catkin_create_pkg simple_goal_pub roscpp rospy actionlib move_base_msgs
创建节点源文件
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
struct goal_pose
{
double pose[3];
double orientation[4];
};
struct goal_pose set_point[]=
{
{
4.0, -4.0, 0, 0, 0, -0.698584, 0.71552},
{
-4.43480, -4.60, 0, 0, 0, 1.0, 0},
{
-5.0280, -1.726740, 0, 0, 0, 0.707358, 0.7068554},
{
0, 0.0, 0, 0, 0, 0, 1 },
/*
/*1米的正方形巡航线，局部定位*/
{
1.0, 0, 0, 0, 0, 0, 1},
{
0.0, 1.0, 0, 0, 0, 1, 0.0},
{
1.0, 0.0, 0, 0, 0, 0, 1},
{
0, 1.0, 0, 0, 0, 1, 0 },
*/
};
int size =sizeof(set_point)/sizeof(goal_pose);
int main(int argc, char *argv[])
{
ros::init(argc, argv, "simple_navigation_goals");
MoveBaseClient ac("move_base",true);
while (!ac.waitForServer(ros::Duration(1.0)))
{
ROS_INFO("waiting for the move_base action server to come up");
}
for(int i=0;i<size;i++)
{
struct goal_pose point=set_point[i];
move_base_msgs::MoveBaseGoal goal;
//goal.target_pose.header.frame_id="base_link"; //局部定位
goal.target_pose.header.frame_id="map"; //全局定位
goal.target_pose.header.stamp=ros::Time::now();
goal.target_pose.pose.position.x=point.pose[0];
goal.target_pose.pose.position.y=point.pose[1];
goal.target_pose.pose.position.z=point.pose[2];
goal.target_pose.pose.orientation.x=point.orientation[0];
goal.target_pose.pose.orientation.y=point.orientation[1];
goal.target_pose.pose.orientation.z=point.orientation[2];
goal.target_pose.pose.orientation.w=point.orientation[3];
ROS_INFO("sending goal ...");
ac.sendGoal(goal);
ROS_INFO("goal executing ...");
ac.waitForResult();
if(ac.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("the base move goal execute succsess ...");
continue;
}
else{
ROS_INFO("the base move goal execute failed ...");
break;
}
}
return 0;
}
因为这个目标点的发布是一个基于action的服务，整个流程应该没有多大问题。
这里之前提醒过，frame_id可以设置成map 就是一个全局定位，设置为base_link 或者base_footprinter 则为一个局部的坐标系，表示以当前机器人位姿作为坐标原点，进行任务点的下发。
通过修改 struct goal_pose set_point里面的节点元素或者顺序，实现机器人按照目标巡航点，依次便利完成。全局定位，需要先使用rviz 点击目标点，然后依次记录下然后修改文件目标点集合。