autoware 之op_behavior_selector（行为选择节点）verticalSafetyDistance、giveUpDistance参数问题探索

最新推荐文章于 2023-07-18 16:53:10 发布

qq_278667286

最新推荐文章于 2023-07-18 16:53:10 发布

阅读量788

点赞数 2

分类专栏： ros 杂谈文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/qq_38288618/article/details/123096040

版权

ros 同时被 2 个专栏收录

64 篇文章 18 订阅

订阅专栏

杂谈

13 篇文章 0 订阅

订阅专栏

autoware 之op_behavior_selector（行为选择节点）verticalSafetyDistance、giveUpDistance参数问题探索

//参数bug

1、verticalSafetyDistance参数

进行局部路径规划时通用参数设置了verticalSafetyDistance 行为选择节点获取参数值表现不正常。
阅读源码，发现有两个参数是从op_trajectory_evaluator空间下取。如下面注释掉的部分

/*****************************/
op_behavior_selector_core.cpp
/*****************************/


void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh)

    //zzz find a bug ??
    //_nh.getParam("/op_trajectory_evaluator/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
    //_nh.getParam("/op_trajectory_evaluator/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
    _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
    _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);

op_trajectory_evaluator节点的launch是有这两个参数的定义：

//
op_trajectory_evaluator.launch
//

<!-- /*****************************/ -->
<!-- op_trajectory_evaluator.launch -->
<!-- /*****************************/ -->
<launch>
	<!-- Trajectory Evaluation Specific Parameters -->
	
	<arg name="enablePrediction" 			default="false" />	 				 		
	<arg name="horizontalSafetyDistance" 	default="1.2" />
	<arg name="verticalSafetyDistance" 		default="0.8" />
			
	<node pkg="op_local_planner" type="op_trajectory_evaluator" name="op_trajectory_evaluator" output="screen">
	
		<param name="enablePrediction" 			value="$(arg enablePrediction)" />						
		<param name="horizontalSafetyDistance" 	value="$(arg horizontalSafetyDistance)" />
		<param name="verticalSafetyDistance" 	value="$(arg verticalSafetyDistance)" />				
			
	</node>				
			
</launch>

但是，关键是这里op_trajectory_evaluator的源码中没有用到自己launch配置的这两个参数，反而是从通用空间下取值。
//
op_trajectory_evaluator_core.cpp
//

void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh)
{
	_nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction);

	_nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
	_nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
	_nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);

于是猜测最上部分获取参数可能是autoware多人开发导致的错误。

//注意问题待分析

2、giveUpDistance参数
红绿灯检测相关参数giveUpDistance为什么为负值

/*****************************/
     DecisionMaker.cpp
/*****************************/
 void DecisionMaker::CalculateImportantParameterForDecisionMaking(const PlannerHNS::VehicleState& car_state,
		 const int& goalID, const bool& bEmergencyStop, const std::vector<TrafficLight>& detectedLights,
		 const TrajectoryCost& bestTrajectory, const bool& loop)
 {
double critical_long_front_distance =  m_CarInfo.wheel_base/2.0 + m_CarInfo.length/2.0 + m_params.verticalSafetyDistance;
//半个车长，半个轴距，和纵向安全距离之和



  	distanceToClosestStopLine = PlanningHelpers::GetDistanceToClosestStopLineAndCheck(m_TotalPath.at(pValues->iCurrSafeLane), state, m_params.giveUpDistance, stopLineID, stopSignID, trafficLightID) - critical_long_front_distance;

    std::cout << "StopLineID: " << stopLineID << ", critical_long_front_distance: " << critical_long_front_distance << ", TrafficLightID: " << trafficLightID << ", giveUpDistance: " << m_params.giveUpDistance << ", Distance: " << distanceToClosestStopLine << ", MinStopDistance: " << pValues->minStoppingDistance + m_params.distanceToCheckingTrafficLight << std::endl;
    //if(distanceToClosestStopLine > m_params.giveUpDistance && distanceToClosestStopLine < (pValues->minStoppingDistance + 0.2))
    if(distanceToClosestStopLine > m_params.giveUpDistance && distanceToClosestStopLine < (pValues->minStoppingDistance + m_params.distanceToCheckingTrafficLight))//zzz 20220223

distanceToClosestStopLine车到停止线的距离。计算上减了critical_long_front_distance即半个车长，半个轴距，和纵向安全距离之和
/op_common_params/giveUpDistance为负数的意义：车中心到停止线距离减了半个车长，是车头到停止线的距离。当车头顶线时，半个轴距和纵向安全距离，就是多减的那部分。

这里GetDistanceToClosestStopLineAndCheck会返回-1为什么不再小一些？没有停止线时让红绿灯检测的条件达不成多好。