本文详细解读了detect_recovery.cpp中的DetectRecovery类，它在firefly机器人导航中用于处理前方障碍时的路径恢复策略。通过初始化、激光数据处理和旋转恢复逻辑，该行为包替代了move_base的原有旋转恢复。
detect_recovery.cpp代码阅读
自己通过在对firefly机器人进行导航时，观察到终端的日志输出出现了有关detect_recovery的信息，这种情况机器人一般是前方遇到了障碍物，伴随着局部路径规划失败的日志信息同时出现的。于是自己通过查看detect_recovery功能包中的源码了解到它是在原先的nav_core::RecoveryBehavior类下派生出的一个类，它来代替原先move_base中的旋转恢复行为。以下是自己对代码的梳理。
- detect_recovery的头文件首先对detect_recovery进行了一些函数的声明和成员属性的定义
namespace detect_recovery
{
/**
* @class RoRecovery
* @brief A recovery behavior that ros the robot in-place to attempt to clear out space
*/
class DetectRecovery : public nav_core::RecoveryBehavior
{
public:
/**
* @brief Constructor, make sure to call initialize in addition to actually initialize the object
*/
DetectRecovery();
/**
* @brief Initialization function for the DetectRecovery recovery behavior
* @param name Namespace used in initialization
* @param tf (unused)
* @param global_costmap (unused)
* @param local_costmap A pointer to the local_costmap used by the navigation stack
*/
void initialize(std::string name, tf2_ros::Buffer*,
costmap_2d::Costmap2DROS*, costmap_2d::Costmap2DROS* local_costmap);
/**
* @brief Run the DetectRecovery recovery behavior.
*/
void runBehavior();
/**
* @brief Destructor for the detect recovery behavior
*/
~DetectRecovery();
private:
ros::NodeHandle *nh_;
costmap_2d::Costmap2DROS* local_costmap_;
bool initialized_;
bool move_base_cancel_;
double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_;
base_local_planner::CostmapModel* world_model_;
double pi;
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
void publishZeroVelocity();
// new add
void getLaserData(sensor_msgs::LaserScan& data);
void getLaserPoint(std::vector< std::pair<double,double> >& data);
void getOdomData(nav_msgs::Odometry& data);
ros::Subscriber laser_sub_;
ros::Subscriber camera_sub_;
ros::Subscriber odom_sub_;
ros::Publisher vel_pub_;
std::vector<std::pair<double,double>> point_vec_;
std::string base_frame_, laser_frame_;
sensor_msgs::LaserScan laser_data_;
nav_msgs::Odometry odom_data_;
boost::mutex laser_mutex_;
boost::mutex odom_mutex_;
tf::StampedTransform transform;
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
void movebaseCancelCallback(const actionlib_msgs::GoalID::ConstPtr& msg);
void getLaserTobaselinkTF(std::string sensor_frame, std::string base_frame);
double inline normalizeAngle(double val, double min, double max)
{
double norm = 0.0;
if (val >= min)
norm = min + fmod((val - min), (max-min));
else
norm = max - fmod((min - val), (max-min));
return norm;
}
};
}; // namespace detect_recovery
#endif // DETECT_RECOVERY_DETECT_RECOVERY_H
接下来对声明的函数进行分析
DetectRecovery::initialize();首先是传入一些参数变量进行初始化操作，通过新开辟一个句柄来对雷达，里程计和向底盘发布速度的节点进行管理和参数的设置。
void DetectRecovery::initialize(std::string name, tf2_ros::Buffer*,
costmap_2d::Costmap2DROS*, costmap_2d::Costmap2DROS* local_costmap)
{
nh_ = new ros::NodeHandle("~/");
laser_sub_ = nh_->subscribe<sensor_msgs::LaserScan>("/scan",1,boost::bind(&DetectRecovery::scanCallback,this,_1));
odom_sub_ = nh_->subscribe<nav_msgs::Odometry>("/odom",1,boost::bind(&DetectRecovery::odomCallback,this,_1));
vel_pub_ = nh_->advertise<geometry_msgs::Twist>("/cmd_vel",1);
base_frame_ = "base_link";
laser_frame_ = "laser";
nh_->setParam("detect_recovery/base_frame", base_frame_);
nh_->setParam("detect_recovery/laser_frame", laser_frame_);
laser_sub_.shutdown();//开始雷达不订阅
odom_sub_.shutdown();//里程计不订阅信息
this->odom_data_.header.stamp = ros::Time::now();//里程计和雷达数据的时间设为当前时刻
this->laser_data_.header.stamp = ros::Time::now();
//设置一些状态
getLaserTobaselinkTF(laser_frame_, base_frame_);
move_base_cancel_ = false;
pi = 3.14;
frequency_ = 10;
initialized_ = true;
}
DetectRecovery::runBehavior()函数是实现恢复行为的主要函数，其中他又调用了一些回调函数
void DetectRecovery::runBehavior()
{
ROS_INFO("---Detect Recovery started---");//提示开始进行恢复行为了
//雷达和里程计开始订阅接受信息
laser_sub_ = nh_->subscribe<sensor_msgs::LaserScan>("/scan",1,boost::bind(&DetectRecovery::scanCallback,this,_1));//调用回调函数scanCallback，主要是获取一些激光数据的信息，并对接收到的激光点进行帅选，剔除一些无用点
odom_sub_ = nh_->subscribe<nav_msgs::Odometry>("/odom",1,boost::bind(&DetectRecovery::odomCallback,this,_1));//调用回调函数odomCallback，获取里程计的数据信息
ros::NodeHandle movebaseCancel_nh;//创建一个取消movebase的节点句柄
ros::Subscriber movebasecancel_sub;//创建一个名为movebasecancel_sub订阅者
//订阅的话题的消息类型为actionlib_msgs::GoalID，话题名为：/move_base/cancel，回调函数movebaseCancelCallback
movebasecancel_sub = movebaseCancel_nh.subscribe<actionlib_msgs::GoalID>("/move_base/cancel",1,
boost::bind(&DetectRecovery::movebaseCancelCallback,this,_1));
//如果有中断请求，则停止恢复
if(move_base_cancel_ == true)
{
publishZeroVelocity();//发布速度为零，使机器人停止
ROS_INFO("---move_base canceled---");
move_base_cancel_ = false;
laser_sub_.shutdown();
odom_sub_.shutdown();
return;
}
//从里程计获取现在的位置和姿态
nav_msgs::Odometry odom_data;
//ros::Duration(0.5).sleep();
this->getOdomData(odom_data);//通过getOdomData()函数获取里程计的当前时刻的位姿信息
double robot_start_x = odom_data.pose.pose.position.x;
double robot_start_y = odom_data.pose.pose.position.y;
double robot_start_t = tf::getYaw(odom_data.pose.pose.orientation);
//接下来进入一个循环的判断，对于得到的里程计的偏航角如果转换不成功，那么进入这个while循环，持续的接受里程计的信息，如果超过了设定的次数（10），则表示机器人当前位姿没有办法得到，这时也不再订阅雷达和里程计的消息，恢复行为也就失败了，退出恢复
uint32_t count_step = 0;
while(isnan(robot_start_t))
{
ROS_INFO("can't get right robot post");
if(count_step++ > 10)
{
laser_sub_.shutdown();
odom_sub_.shutdown();
return;
}
ros::Duration(1).sleep();
this->getOdomData(odom_data);
robot_start_x = odom_data.pose.pose.position.x;
robot_start_y = odom_data.pose.pose.position.y;
robot_start_t = tf::getYaw(odom_data.pose.pose.orientation);
}
//定义现在的位置和姿态
double robot_current_x = robot_start_x;
double robot_current_y = robot_start_y;
double robot_current_t = robot_start_t;
//获取激光数据（这里的逻辑和上面的里程计一样），放到laser_point容器中
std::vector< std::pair<double,double> > laser_point;
this->getLaserPoint(laser_point);//使用回调函数赋值，向容器中存放激光数据
count_step=0;
while(laser_point.size() == 0) //如果激光没有数据，等待10s，如果还没有，退出恢复
{
ROS_INFO("can't get scan data");
if(count_step++ > 10)
{
laser_sub_.shutdown();
odom_sub_.shutdown();
return;
}
ros::Duration(1).sleep();
this->getLaserPoint(laser_point);//使用回调函数赋值
}
size_t laser_data_size = laser_point.size();
//这里设置一些标志位
bool no_point = false;
bool turn_half = false;
bool turn_done = false;
bool goStright = false;
geometry_msgs::Twist cmd_vel;
//这里是不是让机器人向前移动一点点的距离帮助机器人恢复？
while(ros::ok() && !goStright){
while(ros::ok() && turn_done == false)//如果还没有旋转一周，就可以开始恢复了。
{
this->getLaserPoint(laser_point);//获取最新激光数据
size_t index = 0;
for(index=0; index<laser_data_size; index++)
{
if(laser_point[index].first < 0.6 && fabs(laser_point[index].second) < 0.35) //截取前方60cm，左右35cm的数据
break;//发现障碍物，直接跳出遍历循环。
}
if(index == laser_data_size)//如果顺利扫描一圈，则说明没有障碍物
{
no_point = true;
}
cmd_vel.angular.z = no_point == true?0:0.25;//没有障碍物，停下机器人，有就以0.25的速度继续旋转
this->vel_pub_.publish(cmd_vel);
if(no_point == true)
break;//
//获取当前时刻机器人的位姿
this->getOdomData(odom_data);
robot_current_t = tf::getYaw(odom_data.pose.pose.orientation);
//判断恢复行为，机器人是不是旋转了一半
if(fabs(robot_current_t - robot_start_t) > 2)
turn_half = true;//旋转了一半
if(turn_half == true && fabs(robot_current_t-robot_start_t) < 0.1)
{
//确实已经旋转了一半 而且角度差又很小，说明回到了原点。
turn_done = true;
cmd_vel.angular.z = 0;
this->vel_pub_.publish(cmd_vel);//旋转一圈，停止旋转。
}
ros::Duration(0.05).sleep();
ros::spinOnce();
}
//前进逻辑，这里机器人往前面走了一点点的距离（但是distance那里没有看懂）
if(no_point == true)
{
this->getOdomData(odom_data);
robot_current_x = odom_data.pose.pose.position.x;
robot_current_y = odom_data.pose.pose.position.y;
robot_start_x = robot_current_x;
robot_start_y = robot_current_y;
double diff_x = robot_current_x - robot_start_x;
double diff_y = robot_current_y - robot_start_y;
double distance = sqrt(diff_x*diff_x+diff_y*diff_y);
while(ros::ok() && distance < 0.2)
{
//std::cout << "distance: " << distance << std::endl; //debug
this->getLaserPoint(laser_point);
size_t index = 0;
for(index=0;index<laser_point.size();index++)
{
if(laser_point[index].first < 0.4 && fabs(laser_point[index].second) < 0.35)
{
cmd_vel.linear.x = 0;
cmd_vel.angular.z = 0;
this->vel_pub_.publish(cmd_vel);//前方又有了障碍物，先停止
no_point = false;
break;//跳出遍历循环
// turn_done = false;
// turn_half = false;
}
}
//因为判断出了往前直行时，前方0.4长0.35宽的范围内出现了障碍物，就不能再直行了
if(!no_point)
{
break;
}
//再跳出直行循环；
cmd_vel.linear.x = 0.08;
cmd_vel.angular.z = 0;
this->vel_pub_.publish(cmd_vel);
//更新位置
this->getOdomData(odom_data);
robot_current_x = odom_data.pose.pose.position.x;
robot_current_y = odom_data.pose.pose.position.y;
diff_x = robot_current_x - robot_start_x;
diff_y = robot_current_y - robot_start_y;
distance = sqrt(diff_x*diff_x+diff_y*diff_y);
//std::cout << "+++distance: " << distance << std::endl; //debug
if(distance >= 0.2)
{
goStright = true;//说明往前直行了一点
}
ros::Duration(0.05).sleep();
ros::spinOnce();
}
//不再让机器人动
cmd_vel.linear.x = 0;
cmd_vel.angular.z = 0;
this->vel_pub_.publish(cmd_vel);
}
}
ROS_INFO("detect recovery end");//恢复行为结束
laser_sub_.shutdown();
odom_sub_.shutdown();
return;
}
扫一扫
726
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
