一.发布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 <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
/*move_base_msgs::MoveBaseAction
move_base在world中的目标
*/
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 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),机器人会回到原点。