API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Sep 10th, 2019
580
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 13.00 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. * Copyright 2018-2019 Autoware Foundation. All rights reserved.
  3. *
  4. * Licensed under the Apache License, Version 2.0 (the "License");
  5. * you may not use this file except in compliance with the License.
  6. * You may obtain a copy of the License at
  7. *
  8. * http://www.apache.org/licenses/LICENSE-2.0
  9. *
  10. * Unless required by applicable law or agreed to in writing, software
  11. * distributed under the License is distributed on an "AS IS" BASIS,
  12. * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13. * See the License for the specific language governing permissions and
  14. * limitations under the License.
  15. */
  16.  
  17. #include "op_trajectory_evaluator_core.h"
  18. #include "op_ros_helpers/op_ROSHelpers.h"
  19.  
  20.  
  21. namespace TrajectoryEvaluatorNS
  22. {
  23.  
  24. TrajectoryEval::TrajectoryEval()
  25. {
  26. bNewCurrentPos = false;
  27. bVehicleStatus = false;
  28. bWayGlobalPath = false;
  29. bWayGlobalPathToUse = false;
  30. m_bUseMoveingObjectsPrediction = false;
  31.  
  32. ros::NodeHandle _nh;
  33. UpdatePlanningParams(_nh);
  34.  
  35. tf::StampedTransform transform;
  36. PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform);
  37. m_OriginPos.position.x = transform.getOrigin().x();
  38. m_OriginPos.position.y = transform.getOrigin().y();
  39. m_OriginPos.position.z = transform.getOrigin().z();
  40.  
  41. pub_CollisionPointsRviz = nh.advertise<visualization_msgs::MarkerArray>("dynamic_collision_points_rviz", 1);
  42. pub_LocalWeightedTrajectoriesRviz = nh.advertise<visualization_msgs::MarkerArray>("local_trajectories_eval_rviz", 1);
  43. pub_LocalWeightedTrajectories = nh.advertise<autoware_msgs::LaneArray>("local_weighted_trajectories", 1);
  44. pub_TrajectoryCost = nh.advertise<autoware_msgs::Lane>("local_trajectory_cost", 1);
  45. pub_SafetyBorderRviz = nh.advertise<visualization_msgs::Marker>("safety_border", 1);
  46.  
  47. sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this);
  48.  
  49. int bVelSource = 1;
  50. _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource);
  51. if(bVelSource == 0)
  52. sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this);
  53. else if(bVelSource == 1)
  54. sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this);
  55. else if(bVelSource == 2)
  56. sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this);
  57.  
  58. sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this);
  59. sub_LocalPlannerPaths = nh.subscribe("/local_trajectories", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this);
  60. sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this);
  61. sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this);
  62.  
  63. PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy);
  64. }
  65.  
  66. TrajectoryEval::~TrajectoryEval()
  67. {
  68. }
  69.  
  70. void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh)
  71. {
  72. _nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction);
  73.  
  74. _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel);
  75. _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance);
  76. _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving);
  77. if(m_PlanningParams.enableSwerving)
  78. m_PlanningParams.enableFollowing = true;
  79. else
  80. _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing);
  81.  
  82. _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior);
  83. _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior);
  84.  
  85. _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed);
  86. _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed);
  87. _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance);
  88.  
  89. _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity);
  90.  
  91. _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity);
  92. if(m_PlanningParams.enableSwerving)
  93. _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber);
  94. else
  95. m_PlanningParams.rollOutNumber = 0;
  96.  
  97. std::cout << "Rolls Number: " << m_PlanningParams.rollOutNumber << std::endl;
  98.  
  99. _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance);
  100. _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance);
  101. _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
  102. _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
  103. _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
  104.  
  105. _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
  106.  
  107. _nh.getParam("/op_common_params/width", m_CarInfo.width);
  108. _nh.getParam("/op_common_params/length", m_CarInfo.length);
  109. _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base);
  110. _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius);
  111. _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle);
  112. _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration);
  113. _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration);
  114. m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed;
  115. m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed;
  116.  
  117. }
  118.  
  119. void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg)
  120. {
  121. m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation));
  122. bNewCurrentPos = true;
  123. }
  124.  
  125. void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg)
  126. {
  127. m_VehicleStatus.speed = msg->twist.linear.x;
  128. m_CurrentPos.v = m_VehicleStatus.speed;
  129. if(fabs(msg->twist.linear.x) > 0.25)
  130. m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x);
  131. UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
  132. bVehicleStatus = true;
  133. }
  134.  
  135. void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg)
  136. {
  137. m_VehicleStatus.speed = msg->speed/3.6;
  138. m_CurrentPos.v = m_VehicleStatus.speed;
  139. m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value;
  140. UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
  141. bVehicleStatus = true;
  142. }
  143.  
  144. void TrajectoryEval::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg)
  145. {
  146. m_VehicleStatus.speed = msg->twist.twist.linear.x;
  147. m_CurrentPos.v = m_VehicleStatus.speed;
  148. if(fabs(msg->twist.twist.linear.x) > 0.25)
  149. m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x);
  150. UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp);
  151. bVehicleStatus = true;
  152. }
  153.  
  154. void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
  155. {
  156. if(msg->lanes.size() > 0)
  157. {
  158.  
  159. bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size();
  160.  
  161. m_GlobalPaths.clear();
  162.  
  163. for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
  164. {
  165. PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path);
  166.  
  167. PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path);
  168. m_GlobalPaths.push_back(m_temp_path);
  169.  
  170. if(bOldGlobalPath)
  171. {
  172. bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i));
  173. }
  174. }
  175.  
  176. if(!bOldGlobalPath)
  177. {
  178. bWayGlobalPath = true;
  179. std::cout << "Received New Global Path Evaluator! " << std::endl;
  180. }
  181. else
  182. {
  183. m_GlobalPaths.clear();
  184. }
  185. }
  186. }
  187.  
  188. void TrajectoryEval::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
  189. {
  190. if(msg->lanes.size() > 0)
  191. {
  192. m_GeneratedRollOuts.clear();
  193. int globalPathId_roll_outs = -1;
  194.  
  195. for(unsigned int i = 0 ; i < msg->lanes.size(); i++)
  196. {
  197. std::vector<PlannerHNS::WayPoint> path;
  198. PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path);
  199. m_GeneratedRollOuts.push_back(path);
  200. if(path.size() > 0)
  201. globalPathId_roll_outs = path.at(0).gid;
  202. }
  203.  
  204. if(bWayGlobalPath && m_GlobalPaths.size() > 0 && m_GlobalPaths.at(0).size() > 0)
  205. {
  206. int globalPathId = m_GlobalPaths.at(0).at(0).gid;
  207. std::cout << "Before Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
  208.  
  209. if(globalPathId_roll_outs == globalPathId)
  210. {
  211. bWayGlobalPath = false;
  212. m_GlobalPathsToUse = m_GlobalPaths;
  213. std::cout << "Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl;
  214. }
  215. }
  216.  
  217. bRollOuts = true;
  218. }
  219. }
  220.  
  221. void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg)
  222. {
  223. m_PredictedObjects.clear();
  224. bPredictedObjects = true;
  225.  
  226. PlannerHNS::DetectedObject obj;
  227. for(unsigned int i = 0 ; i <msg->objects.size(); i++)
  228. {
  229. if(msg->objects.at(i).id > 0)
  230. {
  231. PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj);
  232. m_PredictedObjects.push_back(obj);
  233. }
  234. // else
  235. // {
  236. // std::cout << " Ego Car avoid detecting itself in trajectory evaluator node! ID: " << msg->objects.at(i).id << std::endl;
  237. // }
  238. }
  239. }
  240.  
  241. void TrajectoryEval::callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr& msg)
  242. {
  243. m_CurrentBehavior.iTrajectory = msg->twist.angular.z;
  244. }
  245.  
  246. void TrajectoryEval::MainLoop()
  247. {
  248. ros::Rate loop_rate(100);
  249.  
  250. PlannerHNS::WayPoint prevState, state_change;
  251.  
  252. while (ros::ok())
  253. {
  254. ros::spinOnce();
  255. PlannerHNS::TrajectoryCost tc;
  256.  
  257. if(bNewCurrentPos && m_GlobalPaths.size()>0)
  258. {
  259. m_GlobalPathSections.clear();
  260.  
  261. for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++)
  262. {
  263. t_centerTrajectorySmoothed.clear();
  264. PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed);
  265. m_GlobalPathSections.push_back(t_centerTrajectorySmoothed);
  266. }
  267.  
  268. if(m_GlobalPathSections.size()>0)
  269. {
  270. if(m_bUseMoveingObjectsPrediction)
  271. tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory);
  272. else
  273. tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects);
  274.  
  275. autoware_msgs::Lane l;
  276. l.closest_object_distance = tc.closest_obj_distance;
  277. l.closest_object_velocity = tc.closest_obj_velocity;
  278. l.cost = tc.cost;
  279. l.is_blocked = tc.bBlocked;
  280. l.lane_index = tc.index;
  281. pub_TrajectoryCost.publish(l);
  282. }
  283.  
  284. if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) // FAILS HERE!!!!!
  285. {
  286. autoware_msgs::LaneArray local_lanes;
  287. for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++)
  288. {
  289. autoware_msgs::Lane lane;
  290. PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane);
  291. lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance;
  292. lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity;
  293. lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost;
  294. lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked;
  295. lane.lane_index = i;
  296. local_lanes.lanes.push_back(lane);
  297. }
  298.  
  299. pub_LocalWeightedTrajectories.publish(local_lanes);
  300. }
  301. else
  302. {
  303. ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()");
  304. }
  305.  
  306. if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0)
  307. {
  308. visualization_msgs::MarkerArray all_rollOuts;
  309. PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts);
  310. pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts);
  311.  
  312. PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy);
  313. pub_CollisionPointsRviz.publish(m_CollisionsActual);
  314.  
  315. //Visualize Safety Box
  316. visualization_msgs::Marker safety_box;
  317. PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box);
  318. pub_SafetyBorderRviz.publish(safety_box);
  319. }
  320. }
  321. else
  322. sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this);
  323.  
  324. loop_rate.sleep();
  325. }
  326. }
  327.  
  328. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!