设定目标点进行导航
注意:
- 再学习本系列教程时,应该已经安装过ROS了并且需要有一些ROS的基本知识
- 此教程以webots_demo为基础
ubuntu版本:20.04
webots版本:2021a
ros版本:noetic
为了能和读者进一步讨论问题,建立了一个微信群,方便给大家解答问题,也可以一起讨论问题。
加群链接
前言
平时,我们都是在Rviz中选中2D Nav Goal
来设置目标点的,这很麻烦。
如果我们想象我们的环境是一个100平的房子,里面有着厨房、卧室、客厅等等,想让机器人自动到达厨房或者卧室,就必须使用程序来解决,如果我们人为在Rviz中设置目标点就不叫自动了。
1. 想法
其实第一时间我是上百度查解决方案,网上教程如出一辙:ROS之用程序设定导航目标点
但是不知为何,笔者在修改参数运行后,move_base会直接崩溃。(未解决)
如出一辙的教程无果后,只能找解决方案。
1.1 另辟蹊径
2D Nav Goal
发布到哪里了,那我直接publish那个topic应该就能达到同样的效果吧。
1.2 测试环境
测试环境如下所示:
2. 测试方法是否可行
启动顺序如下:
roslaunch webots.launch
启动webotsslam_base_gmapping.launch
启动机器人始能检测元件程序、RVIZ、Gmapping和move_base
启动完成后,确保机器人能正常建图。使用rostopic list
查找2D Nav Goal
publish的topic。
最终我找到/move_base_simple/goal
这个topic是2D Nav Goal
publish的topic。
可以使用如下命令查看数据类型。
$ rostopic info /move_base_simple/goal
Type: geometry_msgs/PoseStamped
Publishers:
* /rviz (http://mckros-GL553VD:42121/)
Subscribers:
* /move_base (http://mckros-GL553VD:41431/)
可以看到使用的数据类型为geometry_msgs/PoseStamped
使用如下命令查看数据geometry_msgs/PoseStamped
格式。
$ rosmsg show geometry_msgs/PoseStamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
基本了解过了一些数据类型,和我们应该publish的目标topic,接下来可以在webots中获取坐标点。
3.在webots中获取坐标
我们可以在webots中拖动机器人至想去的地方.
例如,我将机器人拖至主卧室门口,如下图所示:
有两种方法可以查看当前位姿(注意坐标轴):
-
从节点树窗口中找到机器人,查看他的坐标值和旋转轴,如下图所示
-
使用命令
rostopic echo /robot/gps/values
查看机器人的坐标值,如下图所示
使用命令rostopic echo /robot/inertial_unit/quaternion
查看机器人的IMU数据获取旋转角度,如下图所示
从上面获取的数据可以看到当前机器人所处的位姿数据:
pose.position.x = -1.3;
pose.position.y = -2.5; //这里webots中的z轴为rviz中的y轴
pose.orientation.z = 0.0016;
pose.orientation.w = -0.6538;
其他位置可以使用这种方法获取,这里就以这一个位姿进行编程
4.编程
- 创建一个msg名为
Goalname.msg
, 创建一个字符串变量,用于接收用户发送的房间名。
string goal_name
- 配置
CMakeList.txt
add_message_files(
FILES
Goalname.msg
)
- catkin_make编译
- 创建一个名为
robot_set_goals.cpp
的代码文件。将在里面编程。源代码如下(含有注释)
#include "string.h"
#include "webots_demo/Goalname.h" // 自己新建的msg头文件
#include "geometry_msgs/PoseStamped.h"
#include "ros/ros.h"
using namespace std;
ros::NodeHandle *n;
ros::Publisher pub_goal;
void goalCallback(const webots_demo::Goalname::ConstPtr &value);
int main(int argc, char **argv) {
// create a node named 'robot' on ROS network
ros::init(argc, argv, "robot_set_goal");
n = new ros::NodeHandle;
ros::Subscriber sub_goal;
sub_goal = n->subscribe("/robot/goal",1,goalCallback); //订阅用户发布的topic
pub_goal = n->advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",2);
ROS_INFO("Started success ");
ros::spin();
}
void goalCallback(const webots_demo::Goalname::ConstPtr &value){
int isture=0;
string goal_name = value->goal_name;
geometry_msgs::PoseStamped target_pose;
target_pose.header.seq = 1;
target_pose.header.frame_id = "map";
if (goal_name == "bedroom") // 如果用户发送了bedroom
{
target_pose.pose.position.x = -1.3;
target_pose.pose.position.y = -2.5;
target_pose.pose.orientation.z = 0.0016;
target_pose.pose.orientation.w = -0.6538;
isture = 1;
}
if (isture)
{
target_pose.header.stamp = ros::Time::now();
pub_goal.publish(target_pose); // 目标点设置为主卧并且发布目标点
ROS_INFO("Ready to go to the goal %s",goal_name.c_str());
}
else
{
ROS_ERROR("Can't compare the goal");
}
}
- 配置CMakeLists.txt,添加如下程序
add_executable(robot_set_goals src/robot_set_goals.cpp)
add_dependencies(robot_set_goals webots_ros_generate_messages_cpp)
target_link_libraries(robot_set_goals ${catkin_LIBRARIES})
- catkin_make编译
5.测试
- 运行webots
- 运行slam
- 运行robot_set_goals.cpp
- 向/robot/goal 发布信息如下
$ rostopic pub /robot/goal webots_demo/Goalname "goal_name: 'bedroom'"
效果:
可能是角度没搞好,转的方向还需要调整
经过测试,良好的导航算法可以减少误差并且更加平稳。
大家可以发挥想象更好的去利用自动导航程序,比如语音控制。
结语
本文也是基于笔者的学习和使用经验总结的,主观性较强,如果有哪些不对的地方或者不明白的地方,欢迎评论区留言交流~
为了能和读者进一步讨论问题,建立了一个微信群,方便给大家解答问题,也可以一起讨论问题。
加群链接
✌Bye
此项目github地址:https://github.com/JackyMao1999/webots_demo