ROS：使用move_base包发布目标点导航（代码）、tf坐标转换

一.发布map坐标点，导航机器人

1.代码

代码流程：

发布map坐标系目标点，tf转为base_link坐标系下目标点，然后发布MoveBaseGoal，利用move_base导航机器人到达base_link下的目标点。

send_goal.cpp

/**
 * @brief 发布map坐标系目标点，tf转为base_link坐标系下目标点，然后发布MoveBaseGoal，
 *                利用move_base导航机器人到达base_link下目标点
 */
#include 
#include 
#include 
#include 
#include 
/*move_base_msgs::MoveBaseAction
 move_base在world中的目标
*/
typedef actionlib::SimpleActionClient MoveBaseClient;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "send_goals_node");
    tf::TransformListener listener; // 创建tf变换监听者
    // 创建action客户端，参数1：action名，参数2：true，不需要手动调用ros::spin()，会在它的线程中自动调用。
    MoveBaseClient ac("move_base", true);
    // Wait 60 seconds for the action server to become available
    ROS_INFO("Waiting for the move_base action server");
    ac.waitForServer(ros::Duration(60));
    ROS_INFO("Connected to move base server");
    double x=0.,y=0.,z=0.;
    std::cout<<"input x, y, z: ";
    std::cin>>x>>y>>z;

    // 基于map创建目标点
    geometry_msgs::PointStamped map_point;
    map_point.header.frame_id = "map";
    map_point.header.stamp = ros::Time();
    map_point.point.x = x;

    // 将base_link坐标系转为map坐标系
    try
    {
        // 基于map坐标系创建目标点
        geometry_msgs::PointStamped base_point;
        base_point.header.frame_id = "base_link";
        base_point.header.stamp = ros::Time();//此处是ros::Time()，而非ros::Time::now()，两者是有区别的
        ROS_INFO("starting map—>base_link ");
        // 实现map—>base_link，何时转：ros::Time(0)，多久内完成：ros::Duration(3.0)
        listener.waitForTransform("base_link", "map", ros::Time(0), ros::Duration(3.0));
        // "map"为目标坐标系，从base_point转map_point
        listener.transformPoint("base_link", map_point, base_point);
        ROS_INFO("base_link: [x]=%.2f,[y]=%.2f", base_point.point.x, base_point.point.y);

        // 设置目标点信息
        move_base_msgs::MoveBaseGoal goal;
        goal.target_pose.header.frame_id = "base_link"; // map
        goal.target_pose.header.stamp = ros::Time::now();
        goal.target_pose.pose.position = base_point.point;
        double roll = 0.0, pitch = 0.0, yaw = 0.0;                     // 定义欧拉角，可以理解为绕x、y、z轴转动
        geometry_msgs::Quaternion q;                                   // 定义四元数
        q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); // 将欧拉角转为四元数
        goal.target_pose.pose.orientation = q;

        ROS_INFO("Sending goal");
        ac.sendGoal(goal);
        // Wait for the action to return
        ac.waitForResult();
        if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            ROS_INFO("You have reached the goal!");
        else
            ROS_INFO("The base failed for some reason");
    }
    catch (tf::TransformException &ex)
    {
        ROS_ERROR("%s", ex.what());
    }

    return 0;
}

 2.运行

例程：

1、发布map坐标系下目标点（2.0，0），机器人会移动到此处。

2、发布map坐标系下目标点（0，0），机器人会回到原点。

参考

http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals