#ps:许久未更。autoware ROS1教程这些这一两年逐渐多了许多,大多都千篇一律(或许核心内容各自都不公布),虽然手上有一份自己写的教程手册,大家在网上搜搜找找基本上都能找到一点,就不用公布了(60页的内容,主要是懒的公布出来)
正文
回到正文,原始autoware.ai1.13代码中open planner导航方式可能或许应该是不支持连续导航的,在导航到第一个目标点后,需要重新运行全局路径导航节点,局部导航节点以及pure persuit节点才能导航到下一个点。需要我们修改部分代码才能不用重新运行节点直接导航到下一个点。
修改代码
1 op_global_planner_core.cpp的MainLoop()
void GlobalPlanner::MainLoop()
{
if(m_GoalsPos.size() > 0)
{
if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)
{
//修改这个if条件
// if(m_params.bEnableReplanning)
//改成下面的if
if(m_params.bEnableReplanning && m_iCurrentGoalIndex<m_GoalsPos.size()-1)
{
PlannerHNS::RelativeInfo info;
bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info);
if(ret == true && info.iGlobalPath >= 0 && info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size())
{
double remaining_distance = m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);
if(remaining_distance <= REPLANNING_DISTANCE)
{
bMakeNewPlan = true;
if(m_GoalsPos.size() > 0)
m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size();
std::cout << "Current Goal Index = " << m_iCurrentGoalIndex << std::endl << std::endl;
}
}
}
}
}
2 BehaviorStateMachine.cpp的MissionAccomplishedStateII::GetNextState()函数
BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
{
// -------------原有代码--------------------
// return FindBehaviorState(this->m_Behavior);
//需要改成以下的
PreCalculatedConditions* pCParams = GetCalcParams();
if(pCParams->currentGoalID == -1)
return FindBehaviorState(this->m_Behavior);
else
{
pCParams->prevGoalID = pCParams->currentGoalID;
return FindBehaviorState(FORWARD_STATE);
}
}
3. pure_pursuit模块在final_waypoints为空时会出异常
先在pure_pursuit_core.h中添加 ros::Subscriber sub5_;,并增加回调函数声明
private:
// handle
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
std::shared_ptr<autoware_health_checker::HealthChecker> health_checker_ptr_;
// class
PurePursuit pp_;
// publisher
ros::Publisher pub1_, pub2_,
pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_, pub18_;
// subscriber
ros::Subscriber sub1_, sub2_, sub3_, sub4_;
//add下面句话
ros::Subscriber sub5_;
..........
// callbacks
void callbackFromConfig(
const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config);
void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg);
void callbackFromCurrentVelocity(
const geometry_msgs::TwistStampedConstPtr& msg);
void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg_input);
//add下面函数声明
void loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg);
pure_pursuit_core.cpp添加sub5_的定义
void PurePursuitNode::initForROS()
{
.......
// setup subscriber
sub1_ = nh_.subscribe("final_waypoints", 10, &PurePursuitNode::callbackFromWayPoints, this);
sub2_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this);
sub3_ = nh_.subscribe("config/waypoint_follower", 10, &PurePursuitNode::callbackFromConfig, this);
sub4_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this);
sub5_ = nh_.subscribe("loop_waypoints", 10, &PurePursuitNode::loopWaypointsCallback, this); //add
......
}
pure_pursuit_core.cpp 中添加sub5_的回调函数
static autoware_msgs::Lane loopWaypoints;
void PurePursuitNode::loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg)
{
loopWaypoints = *msg;
}
pure_pursuit_core.cpp 中修改callbackFromWayPoints函数,改成以下样子
void PurePursuitNode::callbackFromWayPoints(
const autoware_msgs::LaneConstPtr& msg)
{
autoware_msgs::Lane::Ptr msg_output(new autoware_msgs::Lane);
*msg_output = *msg;
if (!loopWaypoints.waypoints.empty())
{
msg_output->waypoints.insert(msg_output->waypoints.end(), loopWaypoints.waypoints.begin(), loopWaypoints.waypoints.end());
loopWaypoints.waypoints.clear();
}
if (msg_output->waypoints.empty()) return; // 如果waypoints为空,pp_会出异常
if (!msg_output->waypoints.empty())
command_linear_velocity_ = msg_output->waypoints.at(0).twist.twist.linear.x; // 从航点中得到速度
else
command_linear_velocity_ = 0;
pp_.setCurrentWaypoints(msg_output->waypoints);
is_waypoint_set_ = true;
}
最后需要编译一下
AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release