move_base打印輸出LOG如下:
//進入planThread
[ INFO] [1533878479.616677684]: MOVE_BASE makeplan ............//進入makePlan
[ WARN] [1533878479.616771892]: Unable to get starting pose of robot, unable to create global plan
[ INFO] [1533878479.616850642]: move_base_plan_thread No Plan...
[ INFO] [1533878479.616929100]: move_base plan_thread temp_goal:2.127500,0.136710 ////step1
[ INFO] [1533878479.617001725]: MOVE_BASE makeplan ............//進入makePlan//step2
[ WARN] [1533878479.617083684]: Unable to get starting pose of robot, unable to create global plan //進入makePlan//step2
[ INFO] [1533878479.617222225]: move_base_plan_thread No Plan...//退出至planThread//step3
[ INFO] [1533878479.617307100]: move_base plan_thread temp_goal:2.127500,0.136710 //step1
[ INFO] [1533878479.617379142]: MOVE_BASE makeplan ............//進入makePlan//step2
[ WARN] [1533878479.617462850]: Unable to get starting pose of robot, unable to create global plan
......
process[move_base-1]: started with pid [3033]
[ INFO] [1533885354.818131879]: move_base plan_thread //進入planThread
[ INFO] [1533885356.529799005]: Created local_planner base_local_planner/TrajectoryPlannerROS //創建一個局部規劃器
[ WARN] [1533885604.047814331]: Costmap2DROS transform timeout. Current time: 1533885604.0476, global_pose stamp: 1533885602.5442, tolerance: 1.5000
[ WARN] [1533885604.076749998]: Could not get robot pose, cancelling reconfiguration
現象描述:
在項目中設定的流程爲先執行move_base,到達目標後執行沿邊清掃程序,以上故障會導致定點移動失敗!且沿邊時會碰撞障礙物並不停止。
“Unable to get starting pose of robot, unable to create global plan”
逐本溯源:
逐本1層,以上log源於move_base.cpp函數,以上log是在運行planTread線程,進行規劃makePlan並不斷失敗,重新規劃的過程!細節見下:
bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
//make sure to set the plan to be empty initially
plan.clear();
//since this gets called on handle activate
if(planner_costmap_ros_ == NULL) { //全局導航的地圖
ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
return false;
}
//get the starting pose of the robot
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
return false;
}
geometry_msgs::PoseStamped start;
tf::poseStampedTFToMsg(global_pose, start);//
//if the planner fails or returns a zero length plan, planning failed
if(!planner_->makePlan(start, goal, plan) || plan.empty()){
ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
return false;
}
return true;
}
bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan)中,該函數①清除了規劃器plan --->②全局導航的地圖是否爲空--->③獲取設備在全局導航地圖中的座標--->④將該座標轉換爲move_base中的座標geometry_msgs::PoseStamped作爲起點--->⑤根據起點和目標點規劃plan;
根據log可知,排除了全局導航的地圖爲空的可能性,問題在於getRobotPose失敗!當且僅當planner_costmap_ros_->getRobotPose(global_pose)值爲0時。
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
return false;
逐本2層,etRobotPose這個函數是costmap_2d包中costmap_2d_ros.cpp,源碼如下:
bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
global_pose.setIdentity();
tf::Stamped < tf::Pose > robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
// get the global pose of the robot
try
{
tf_.transformPose(global_frame_, robot_pose, global_pose);
}
catch (tf::LookupException& ex)
{
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
return false;
}
// check global_pose timeout
if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
{
ROS_WARN_THROTTLE(1.0,
"Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
return false;
}
return true;
}
這是一個函數實現了座標的tf轉換,並且進行了異常處理(作者早期的源碼只有左邊轉換,無異常處理);
有四種情況下會輸出false,分別是LookupException、ConnectivityException、ExtrapolationException、global_pose timeout;
其中前三個是由異常導致!!!最後一個是由於當前時間和global_pose時間差大於容忍轉換時間導致!!!
從try程序塊tf_.transformPose()可知,getRobotPose進行的工作是tf轉換!!!
將robot_pose轉換成global_pose,根據我們對tf轉換的瞭解可知,這兩者數據類型相同;(下面有分析均爲Stamped<tf::Pose>)
因此造成以上失敗的原因,可能有兩種!
- tf_.transformPose轉換異常
- tf_.transformPose超時
以上兩種情況,按理論上應該是有打印輸出,通過查看打印就能知道是哪種原因造成的。但實際運行時,由於日誌級別設置問題,不會輸出ROS_WARN_THROTTLE相關信息!(此處有坑,理論上warn和error級別高於info,應該有打印輸出,實際輸出的log並未看見相關信息)。正常情況下,如果有以上四種LOG中的任意一個就可以定位問題是轉換異常還是轉換超時,但是目前我們缺乏信息,只能進一步深挖了!
補充信息:tf_.transformPose轉換異常的類型彙總
tf/include/tf/exceptioms.h
#ifndef TF_EXCEPTIONS_H
#define TF_EXCEPTIONS_H
#include <stdexcept>
#include <tf2/exceptions.h>
namespace tf{
// Pass through exceptions from tf2
typedef tf2::TransformException TransformException;
typedef tf2::LookupException LookupException;
typedef tf2::ConnectivityException ConnectivityException;
typedef tf2::ExtrapolationException ExtrapolationException;
typedef tf2::InvalidArgumentException InvalidArgument;
}
#endif //TF_EXCEPTIONS_H
逐本3層,如果是tf_.transformPose轉換異常,查詢tf_.transformPose具體內容:
那麼我們查詢下函數transformPose的位置在tf包中(有兩處):
- 第一處位於:
tf/include/tf.h描述了函數的作用和用法:
/** \brief Transform a Stamped Pose into the target frame
* This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/
void transformPose(const std::string& target_frame, const Stamped<tf::Pose>& stamped_in, Stamped<tf::Pose>& stamped_out) const;
tf/src/tf.cpp對函數進行了實體化
void Transformer::transformPose(const std::string& target_frame, const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const
{
StampedTransform transform;
lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
stamped_out.setData(transform * stamped_in);
stamped_out.stamp_ = transform.stamp_;
stamped_out.frame_id_ = target_frame;
};
- 第二處位於:
tf/include/tf/transform_listener.h描述了函數的作用和用法:
/** \brief Transform a Stamped Pose Message into the target frame
* This can throw all that lookupTransform can throw as well as tf::InvalidTransform */
void transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& stamped_in, geometry_msgs::PoseStamped& stamped_out) const;
tf/src/transform_listener.cpp對函數進行了實體化
void TransformListener::transformPose(const std::string& target_frame,
const geometry_msgs::PoseStamped& msg_in,
geometry_msgs::PoseStamped& msg_out) const
{
tf::assertQuaternionValid(msg_in.pose.orientation);
Stamped<Pose> pin, pout;
poseStampedMsgToTF(msg_in, pin);
transformPose(target_frame, pin, pout); //
poseStampedTFToMsg(pout, msg_out);
}
兩處函數名雖然相同,形參的數據類型不同,根據查看我們的代碼中形參的數據類型(tf::Stamped<tf::Pose>& global_pose) 可知,使用的是前者!transformPose中實際處理數據的函數是lookupTransform和setData;
逐本4層,什麼情況下導致以上提到的異常呢?lookupTransform()函數定義
/*********** Accessors *************/
/** \brief Get the transform between two frames by frame ID.
* \param target_frame The frame to which data should be transformed
* \param source_frame The frame where the data originated
* \param time The time at which the value of the transform is desired. (0 will get the latest)
* \param transform The transform reference to fill.
*
* Possible exceptions tf::LookupException, tf::ConnectivityException,
* tf::MaxDepthException, tf::ExtrapolationException
*/
void lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, StampedTransform& transform) const;
void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, StampedTransform& transform) const
{
geometry_msgs::TransformStamped output =
tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame),
strip_leading_slash(source_frame), time);
transformStampedMsgToTF(output, transform);
return;
};
tf2_buffer_ptr_結合tf_.transformPose轉換異常的類型#include <tf2/exceptions.h>可知,lookupTransform依賴於tf2包!說實在的我都快暈了,戰線越來越長!(TF2是用來替換tf的,尤其是在Migration這一篇說到:tf已經被拋棄啦!)
<留坑符>暫時跳出來,分析超時問題!
2.轉換超時該如何處理?
答案:增大transform_tolerance_
costmap_2d_ros.cpp中對transform_tolerance_參數的默認設置爲0.3;
transform_tolerance_(0.3),
如何修改transform_tolerance_呢?兩種方式一種,一種通過yaml更新節點參數,一種通過cfg動態調參;
第一種方法:
修改local_costmap_params.yaml中參數transform_tolerance的值
local_costmap:
# global_frame: /odom
global_frame: /odom_combined
#global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 3.0
height: 3.0
resolution: 0.05
transform_tolerance: 3 //transform_tolerance的調參範圍0.3~10,由1.5改爲3
map_type: costmap
再通過roslaunch參數配置rosparam加載load進去
<?xml version="1.0"?>
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find atrobot_nav)/config/lqc_test/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find atrobot_nav)/config/lqc_test/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find atrobot_nav)/config/lqc_test/local_costmap_params.yaml" command="load" />
<rosparam file="$(find atrobot_nav)/config/lqc_test/global_costmap_params.yaml" command="load" />
<rosparam file="$(find atrobot_nav)/config/lqc_test/base_local_planner_params.yaml" command="load" />
<rosparam file="$(find atrobot_nav)/config/lqc_test/dwa_local_planner_params.yaml" command="load" />
<rosparam file="$(find atrobot_nav)/config/lqc_test/base_global_planner_params.yaml" command="load" />
</node>
</launch>
如何查看是否修改成功?在終端通過rosparam指令查看!《Using rosparam查看和設置參數》
rosparam list
rosparam get /move_base/global_costmap/transform_tolerance
這兩個結果分別說明修改生效了!~(從以上代碼也可以看出gloal_costmap和local_costmap都調用的同一段程序!)
第二種方法:動態調參
// Else neither param was found anywhere this knows about, so
// defaults will come from dynamic_reconfigure stuff, set in
// cfg/Costmap2D.cfg and read in this file in reconfigureCB().
當參數服務器得到重新配置參數的請求,就會調用回調函數。即需要動態調參客戶端client,發送重新配置參數的請求時,纔會從cfg中加載配置參數!
兩種的差別:
參考文獻:
move_base warning: "Unable to get starting pose of robot, unable to create global plan"
ROS naviagtion analysis: costmap_2d--Costmap2DROS
https://blog.csdn.net/u013158492/article/details/50485418
Costmap2DROS transform timeout.Could not get robot pose, cancelling reconfiguration
ROS naviagtion analysis: costmap_2d--Costmap2DROS
https://blog.csdn.net/u013158492/article/details/50485418
ROS動態調參(dynamic reconfigure)客戶端服務端之C++ Python實現
https://blog.csdn.net/u014610460/article/details/79531616
ROS_Dynamic Reconfig 動態參數調節
https://blog.csdn.net/CSDNhuaong/article/details/78607184
ROS與C++入門教程-Logging(日誌)
https://www.ncnynl.com/archives/201702/1299.html?tdsourcetag=s_pctim_aiomsg
使用rosconsole和設置顯示級別
https://blog.csdn.net/Siyuada/article/details/81976229?utm_source=blogxgwz1
tf源碼學習--tf