使用move_base做4个点循环跑的导航
突然间接到个任务,要用ROS做一个循环跑的导航小车。这有点难住我了,以前用move_base跑导航的时候只是给一个目标点然后等它走过去,但是怎么才能循环的跑起来呢?第一反应百度一下, 好吧,貌似网上没有用move_base跑循环的。。。那就自己写一个吧。以前记得《ros by example 1》里有个用move_base跑4个目标点的程序,先去看了下源码,发现它仅仅是把目标点发送出去,...
突然间接到个任务,要用ROS做一个循环跑的导航小车。这有点难住我了,以前用move_base跑导航的时候只是给一个目标点然后等它走过去,但是怎么才能循环的跑起来呢?第一反应百度一下, 好吧,貌似网上没有用move_base跑循环的。。。那就自己写一个吧。
以前记得《ros by example 1》里有个用move_base跑4个目标点的程序,先去看了下源码,发现它仅仅是把目标点发送出去,然后等待move_base 60s,如果这段时间内成功完成导航任务,则继续发布下个目标点,否则退出。程序源码在这!https://github.com/pirobot/rbx1/blob/indigo-devel/rbx1_nav/nodes/move_base_square.py
def move(self, goal):
# Send the goal pose to the MoveBaseAction server
self.move_base.send_goal(goal)
# Allow 1 minute to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))
# If we don't get there in time, abort the goal
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
# We made it!
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
当时觉得好无语,哪有做控制是靠等待时间来判断的啊,通过当前位姿与目标点位姿的距离才能判断出导航任务是否完成啊。所以,只要把这段代码中添加一个距离的判断就可以实现基于move_base的循环导航了。即:
if(distance < 1.0)//米
{
move_base.send_goal(goal);
}
只要当前位姿与目标点位姿的距离小于1米了,我就重新发送一个目标点给move_base,这样就可以实现循环导航了(在实验时试过:在导航过程中重新发送目标点,机器人会重新规划处一条到新目标点的路径及轨迹)。那当前时刻的位姿怎么确定呢?有2种方式,一种是订阅 定位 节点发布的 位姿 话题;感觉这种有点low,就pass掉了,因为我觉得应该会有一种更优越的方法。第二种稍后揭晓。
Actionlib
为了寻找这种更优的方法,只有去看move_base的源码了!以前只知道move_base加载全局路径规划和局部路径规划,然后使用局部路径规划来计算出cmd_vel,一直都没去看move_base的源码。。。
看了代码发现move_base其实是actionlib的服务端的实现,好吧,不知道actionlib是啥。后来又去看actionlib的概念。action也是一种类似于service的问答通讯机制,不一样的地方是action还带有一个反馈机制,可以不断反馈任务的实施进度,而且可以在任务实施过程中,中止运行。哇,带反馈,这不就是我要的更优越的方法么。之后通过下面2位大神的文章以及ROS官网中的教程简单学习了一下actionlib的使用。
https://www.cnblogs.com/cv-pr/p/6005013.html
http://wiki.ros.org/actionlib/Tutorials
如下图可以看出,action分为服务端和客户端,服务端会不断的向客服端发送反馈信息,而move_base为服务端,上述《ros by example 1 》中的例子为客服端。所以只需要在客户端中使用这个反馈就可以了。那么,怎么实现呢?
想要使用这个反馈需要确定3件事情:
1 确定move_base是否将这个反馈信息发出来了;
2 通过学习actionlib知道了这个消息传递的消息类型是要自己定义的,那么move_base反馈的消息类型是什么呢?
3 怎么才能接受到这个反馈消息呢?
问题1:是否发出了反馈消息
我们先从第一个问题开始:通过下面这个文章,知道了move_base确实是发布了反馈信息,而且反馈信息就是当前的位姿。ok,成功解决第一个问题。注意,这个反馈消息不是move_base发出来的,而是actionlib发出来的!
https://www.cnblogs.com/shhu1993/p/6323699.html
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
//发布速度topic
geometry_msgs::Twist cmd_vel;
//push the feedback out
//发布一些反馈信息
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position = current_position;
as_->publishFeedback(feedback);
//省略。。。
}
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal){
//we have a goal so start the planner(通知planner线程进行路径规划)
boost::unique_lock<boost::mutex> lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
#通知规划路径线程
planner_cond_.notify_one();
lock.unlock();
ros::NodeHandle n;
while(n.ok())
{
#被抢占了(可能是发出新的goal,也可能是取消了)
if(as_->isPreemptRequested()){
if(as_->isNewGoalAvailable()){
#发布新的goal,通知planner线程工作。
planner_cond_.notify_one();
}
else{
//if we've been preempted explicitly we need to shut things down
//强制退出
return;
}
}
//省略。。。
}
}
与此同时,我也看到了在新的目标点到来时,新的任务抢占了之前的任务,move_base会将新的目标点发送到actionlib中并重新规划路径。
问题2:消息类型
move_base的action通讯的消息类型在安装move_base时会自动安装,ROS中(http://wiki.ros.org/move_base_msgs)提供了这个消息的说明。简单说明一下,消息类型是由2个“---”分隔开的三种数据类型组成。第一条为目标点的消息类型;第二条为当此次动作执行完成时向客户端发送的消息类型,仅在动作结束时发送一次;第三条为反馈的消息类型,这个是在动作执行过程中一直发送的。
在这里可以看到,move_base的反馈消息类型为 geometry_msgs/PoseStamped ,消息的名字为 base_position。
#goal definition
geometry_msgs/PoseStamped target_pose
---
#result definition
---
#feedback
geometry_msgs/PoseStamped base_position
这个反馈消息是发送在/move_base命名空间下的/feedback话题上,也就是/move_base/feedback。通过接收这个话题得到的消息如下所示:
---
header:
seq: 640
stamp:
secs: 1547635103
nsecs: 592393429
frame_id: ''
status:
goal_id:
stamp:
secs: 1547635097
nsecs: 258239984
id: "/nav_test-4-1547635097.258"
status: 1
text: "This goal has been accepted by the simple action server"
feedback:
base_position:
header:
seq: 0
stamp:
secs: 1547635103
nsecs: 550548076
frame_id: "map"
pose:
position:
x: 0.00729128714117
y: 0.0192558593724
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.0199257022983
w: 0.999801463486
---
OK,第二个问题成功解决掉。
问题3:怎么接收反馈消息
在学习actionlib时知道了actionlib的使用方式,即,一方为服务端,负责发送动作执行的状态与结果;一方为客户端,负责发送动作目标并监听动作状态。反馈消息是从 服务端 按照着 自己定义的消息类型 向客户端发出。所以,反馈消息的处理是在客服端内进行,通过回调函数的方式进行处理。ROS中有例子如下:
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib_tutorials/FibonacciAction.h>
using namespace actionlib_tutorials;
typedef actionlib::SimpleActionClient<FibonacciAction> Client;
// Called once when the goal completes
void doneCb(const actionlib::SimpleClientGoalState& state,
const FibonacciResultConstPtr& result)
{
ROS_INFO("Finished in state [%s]", state.toString().c_str());
ROS_INFO("Answer: %i", result->sequence.back());
ros::shutdown();
}
// Called once when the goal becomes active
void activeCb()
{
ROS_INFO("Goal just went active");
}
// Called every time feedback is received for the goal
void feedbackCb(const FibonacciFeedbackConstPtr& feedback)
{
ROS_INFO("Got Feedback of length %lu", feedback->sequence.size());
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "test_fibonacci_callback");
// Create the action client
Client ac("fibonacci", true);
ROS_INFO("Waiting for action server to start.");
ac.waitForServer();
ROS_INFO("Action server started, sending goal.");
// Send Goal
FibonacciGoal goal;
goal.order = 20;
ac.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
ros::spin();
return 0;
}
可以看到,通过在发送目标点时 将回调函数注册进去,从而在每次反馈消息到来时将自动调用回调函数,也就是上文中的feedbackCb()函数。在actionlib的源码中可以找到sendGoal()的声明,它为后面三个回调函数给了默认的初始值,一个指向空函数的函数指针。
/**
* \brief Sends a goal to the ActionServer, and also registers callbacks
*
* If a previous goal is already active when this is called. We simply forget
* about that goal and start tracking the new goal. No cancel requests are made.
* \param done_cb Callback that gets called on transitions to Done
* \param active_cb Callback that gets called on transitions to Active
* \param feedback_cb Callback that gets called whenever feedback for this goal is received
*/
void sendGoal(const Goal & goal,
SimpleDoneCallback done_cb = SimpleDoneCallback(),
SimpleActiveCallback active_cb = SimpleActiveCallback(),
SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());
知道了回调函数的使用方式就可以接受反馈消息了。不完整的几行代码如下:在下面的代码中,每次反馈消息的到来都会调用feedbackCb(),并将base_position赋值到全局变量current_point当中。
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> Client;
geometry_msgs::Point current_point;
// Called once when the goal becomes active
void activeCb()
{
ROS_INFO("Goal Received");
}
// Called every time feedback is received for the goal
void feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
{
// ROS_INFO("Got base_position of Feedback");
current_point.x = feedback->base_position.pose.position.x;
current_point.y = feedback->base_position.pose.position.y;
current_point.z = feedback->base_position.pose.position.z;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "nav_move_base");
ros::NodeHandle node;
//Subscribe to the move_base action server
Client ac("move_base", true);
//Publisher to manually control the robot (e.g. to stop it, queue_size=5)
cmdVelPub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 5);
move_base_msgs::MoveBaseGoal goal;
//Use the map frame to define goal poses
goal.target_pose.header.frame_id = "map";
//Set the time stamp to "now"
goal.target_pose.header.stamp = ros::Time::now();
//Set the goal pose to the i-th waypoint
goal.target_pose.pose = pose_list[count];
//Start the robot moving toward the goal
ac.sendGoal(goal, Client::SimpleDoneCallback(), &activeCb, &feedbackCb);
return 0;
}
可以看到,在sendGoal()函数中,第一个回调函数按照默认的声明给定了空值,并将在客户端中实现的后两个回调函数注册到actionlib中。
ac.sendGoal(goal, Client::SimpleDoneCallback(), &activeCb, &feedbackCb);
这个方法使用了全局函数,但是假如我想使用类该怎么做呢?这就需要boost库中的bind了,用法如下:
actionlib::SimpleActionClient<FibonacciAction> ac;
// Need boost::bind to pass in the 'this' pointer
ac.sendGoal(goal,
boost::bind(&MyNode::doneCb, this, _1, _2),
Client::SimpleActiveCallback(),
Client::SimpleFeedbackCallback());
OK,至此,我们已经成功解决上述3个问题并且成功的接受到了作为反馈消息的当前位姿。有了当前位姿我们就可以做很多事情了,包括编写一个基于move_base的循环跑的小程序。
虽然上述代码不完整,但是我想聪明的你肯定能够依照这上述的例子实现自己的程序。当然,如果你想下载我的完整代码也是可以的,不过我很不厚道的收费了,嘿嘿。下载链接如下:
https://download.csdn.net/download/tiancailx/10935014
程序用法:
roslaunch rbx1_bringup fake_turtlebot.launch
roslaunch hit_nav fake_move_base_blank_map.launch
rviz
rosrun hit_nav move_base_square_hit
第一步为启动基于arbotix模拟的仿真模型,使用的是《ros by example 1 》的代码,github链接如下:https://github.com/pirobot/rbx1,并且需要安装arbotix 以及 move_base。相信聪明的你一定能够运行成功的。
rviz的配置如下:
运行时的截图。由于目标点间距设置的比较近,所以有时即使改变了目标点他也会先完成之前的一个任务,把目标点设置远些、距离目标点的阈值设置的大些,就可以解决这个问题。
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)