dwa_local_planner 코드분석

[1701279 이승헌]

dwa_planner_ros.cpp

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307

#include <dwa_local_planner/dwa_planner_ros.h> #include <Eigen/Core> #include <cmath>   #include <ros/console.h>   #include <pluginlib/class_list_macros.h>   #include <base_local_planner/goal_functions.h> #include <nav_msgs/Path.h> #include <tf2/utils.h>   #include <nav_core/parameter_magic.h>   //register this planner as a BaseLocalPlanner plugin PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)   //전체 순서 : 새 경로를 계획할 때 setPlan을 사용하여 DWAPlanner에 경로를 전달 //목표지점에 도달했는지 확인 후, 도달하지 못했으면 computeVelocityCommands 함수 사용 //computeVelocityCommands 함수에서 목표지점에 도착했는지 확인 못했으면 dwacompute~ namespace dwa_local_planner {     //local planner의 매개변수를 업데이트하기 위한 콜백함수   void DWAPlannerROS::reconfigureCB(DWAPlannerConfig &config, uint32_t level) {       if (setup_ && config.restore_defaults) {         config = default_config_;         config.restore_defaults = false;       }       if ( ! setup_) {         default_config_ = config;         setup_ = true;       }         // update generic local planner params       base_local_planner::LocalPlannerLimits limits;       limits.max_vel_trans = config.max_vel_trans;       limits.min_vel_trans = config.min_vel_trans;       limits.max_vel_x = config.max_vel_x;       limits.min_vel_x = config.min_vel_x;       limits.max_vel_y = config.max_vel_y;       limits.min_vel_y = config.min_vel_y;       limits.max_vel_theta = config.max_vel_theta;       limits.min_vel_theta = config.min_vel_theta;       limits.acc_lim_x = config.acc_lim_x;       limits.acc_lim_y = config.acc_lim_y;       limits.acc_lim_theta = config.acc_lim_theta;       limits.acc_lim_trans = config.acc_lim_trans;       limits.xy_goal_tolerance = config.xy_goal_tolerance;       limits.yaw_goal_tolerance = config.yaw_goal_tolerance;       limits.prune_plan = config.prune_plan;       limits.trans_stopped_vel = config.trans_stopped_vel;       limits.theta_stopped_vel = config.theta_stopped_vel;       planner_util_.reconfigureCB(limits, config.restore_defaults);         // update dwa specific configuration       dp_->reconfigure(config);   }     DWAPlannerROS::DWAPlannerROS() : initialized_(false),       odom_helper_("odom"), setup_(false) {     }      //초기화   void DWAPlannerROS::initialize(       std::string name, //궤적 플래너의 이 인스턴스에 부여할 이름       tf2_ros::Buffer* tf, //변환 수신기에 대한 포인터       costmap_2d::Costmap2DROS* costmap_ros) //궤적에 비용을 할당하는 데 사용할 비용 맵   {     if (! isInitialized()) {         ros::NodeHandle private_nh("~/" + name); //ros 시스템과 통신을 위한 노드 핸들선언       g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);        //퍼블리셔 선언, 토픽명은 "globla_plan", 큐 사이즈 1개로 설정       l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);       //퍼블리셔 선언, 토픽명은 "local_plan", 큐 사이즈 1개로 설정       tf_ = tf;       costmap_ros_ = costmap_ros;       costmap_ros_->getRobotPose(current_pose_); //전체 좌표와 자세를 가져옴(bool타입)         costmap_2d::Costmap2D* costmap = costmap_ros_->getCostmap();         planner_util_.initialize(tf, costmap, costmap_ros_->getGlobalFrameID());         //create the actual planner that we'll use.. it'll configure itself from the parameter server       dp_ = boost::shared_ptr<DWAPlanner>(new DWAPlanner(name, &planner_util_));         if( private_nh.getParam( "odom_topic", odom_topic_ ))       {         odom_helper_.setOdomTopic( odom_topic_ );       }              initialized_ = true;         // Warn about deprecated parameters -- remove this block in N-turtle       nav_core::warnRenamedParameter(private_nh, "max_vel_trans", "max_trans_vel");       nav_core::warnRenamedParameter(private_nh, "min_vel_trans", "min_trans_vel");       nav_core::warnRenamedParameter(private_nh, "max_vel_theta", "max_rot_vel");       nav_core::warnRenamedParameter(private_nh, "min_vel_theta", "min_rot_vel");       nav_core::warnRenamedParameter(private_nh, "acc_lim_trans", "acc_limit_trans");       nav_core::warnRenamedParameter(private_nh, "theta_stopped_vel", "rot_stopped_vel");         dsrv_ = new dynamic_reconfigure::Server<DWAPlannerConfig>(private_nh);       dynamic_reconfigure::Server<DWAPlannerConfig>::CallbackType cb        = boost::bind(&DWAPlannerROS::reconfigureCB, this, _1, _2);       dsrv_->setCallback(cb);     }     else{       ROS_WARN("This planner has already been initialized, doing nothing.");     }   }      //전역 경로 설정(설정이 성공이면 ture, 실패면 false)   bool DWAPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {     if (! isInitialized()) {       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");       return false;     }     //when we get a new plan, we also want to clear any latch we may have on goal tolerances     latchedStopRotateController_.resetLatching(); //latch 상관관계 재설정       ROS_INFO("Got new plan");     return dp_->setPlan(orig_global_plan); //경로 전달   }      //목표 지점에 도달했는지 여부를 결정(모션 계획 전에 확인)   bool DWAPlannerROS::isGoalReached() {     if (! isInitialized()) {//플래너가 초기화되지 않으면       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");       return false;     }     if ( ! costmap_ros_->getRobotPose(current_pose_)) { //전체 좌표와 자세를 받아오지 못하면       ROS_ERROR("Could not get robot pose");       return false;     }       if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) {       ROS_INFO("Goal reached");       return true;     } else {       return false;     }     //위에 reconfigureCB에서 설정한값들로 비교해서 위치가 xy_goal_tolerance 내에 있고,     //방향이 yaw_goal_tolerance 내에 있는지,     //속도가 trans_stopped_velocity, rot_stopped_velocity보다 작으면 제어 종료     //즉, 0 속도를 전송하고 move_base 관련 제어 플래그 리셋 후 액션 결과 리턴.     //그렇지 않으면 computeVelocityCommands함수를 사용하여 로컬 계획 속도 계산   }     void DWAPlannerROS::publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path) {     base_local_planner::publishPlan(path, l_plan_pub_);   }       void DWAPlannerROS::publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path) {     base_local_planner::publishPlan(path, g_plan_pub_);   }     DWAPlannerROS::~DWAPlannerROS(){     //make sure to clean things up     delete dsrv_;   }       //로봇의 현재 위치, 방향 및 속도가 주어지면, dwa 알고리즘 이용하여 보낼 속도 명령 계산   //cmd_vel : 로봇에 전달할 속도 명령   // return : 유효한 궤적을 찾으면 True, 그렇지 않으면 False   bool DWAPlannerROS::dwaComputeVelocityCommands   (geometry_msgs::PoseStamped &global_pose, geometry_msgs::Twist& cmd_vel) {     // dynamic window sampling approach to get useful velocity commands     if(! isInitialized()){       ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");       return false;     }       geometry_msgs::PoseStamped robot_vel;     odom_helper_.getRobotVel(robot_vel);       /* For timing uncomment     struct timeval‎ start, end;     double start_t, end_t, t_diff;     gettimeofday(&start, NULL);     */       //어느 궤적을 따라갈지 정함     geometry_msgs::PoseStamped drive_cmds;     drive_cmds.header.frame_id = costmap_ros_->getBaseFrameID();          // DWAPlanner::findBestPath 참조     base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds);     //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_);       /* For timing uncomment     gettimeofday(&end, NULL);     start_t = start.tv_sec + double(start.tv_usec) / 1e6;     end_t = end.tv_sec + double(end.tv_usec) / 1e6;     t_diff = end_t - start_t;     ROS_INFO("Cycle time: %.9f", t_diff);     */       //pass along drive commands     cmd_vel.linear.x = drive_cmds.pose.position.x;     cmd_vel.linear.y = drive_cmds.pose.position.y;     cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);       //최적의 궤적 비용값이 0보다 작으면 flase 반환 재계획     std::vector<geometry_msgs::PoseStamped> local_plan;     if(path.cost_ < 0) {       ROS_DEBUG_NAMED("dwa_local_planner",           "The dwa local planner failed to find a valid plan,            cost functions discarded all candidates.            This can mean there is an obstacle too close to the robot.");       local_plan.clear();       publishLocalPlan(local_plan);       return false;     }       ROS_DEBUG_NAMED("dwa_local_planner",      "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.",                      cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);       // Fill out the local plan     for(unsigned int i = 0; i < path.getPointsSize(); ++i) {       double p_x, p_y, p_th;       path.getPoint(i, p_x, p_y, p_th);         geometry_msgs::PoseStamped p;       p.header.frame_id = costmap_ros_->getGlobalFrameID();       p.header.stamp = ros::Time::now();       p.pose.position.x = p_x;       p.pose.position.y = p_y;       p.pose.position.z = 0.0;       tf2::Quaternion q;       q.setRPY(0, 0, p_th);       tf2::convert(q, p.pose.orientation);       local_plan.push_back(p);     }       //publish information to the visualizer       publishLocalPlan(local_plan);     return true;   }      //로봇의 현재 위치, 방향 및 속도가 주어지면, 보낼 속도 명령 계산(전역경로)   //cmd_vel : 로봇에 전달할 속도 명령   // return : 유효한 궤적을 찾으면 True, 그렇지 않으면 False   bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {     //전체 자표와 자세를 받아옴     if ( ! costmap_ros_->getRobotPose(current_pose_)) {       ROS_ERROR("Could not get robot pose");       return false;     }     //로봇의 현재좌표를 기준으로 전역경로를 구함     std::vector<geometry_msgs::PoseStamped> transformed_plan;     if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {       ROS_ERROR("Could not get local plan");       return false;     }       //매핑된 global plan이 비어있으면 flase(매핑이 되지 않았다는 뜻)     if(transformed_plan.empty()) {       ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");       return false;     }     ROS_DEBUG_NAMED("dwa_local_planner",      "Received a transformed plan with %zu points.", transformed_plan.size());       //전역 경로가 로컬 맵에 매핑됨     dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan, costmap_ros_->getRobotFootprint());       //isPositionReachd : 목표 위치에 도달했는지 확인     if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {       //publish an empty plan because we've reached our goal position       std::vector<geometry_msgs::PoseStamped> local_plan;       std::vector<geometry_msgs::PoseStamped> transformed_plan;       publishGlobalPlan(transformed_plan);       publishLocalPlan(local_plan);       base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();       //computeVelocityCommandsStopRotate 함수를 사용하여 해당 감속 정지를 계산하거나       //목표 방향 속도 명령으로 회전       return latchedStopRotateController_.computeVelocityCommandsStopRotate(           cmd_vel,           limits.getAccLimits(),           dp_->getSimPeriod(),           &planner_util_,           odom_helper_,           current_pose_,           boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));     }     //목표위치에 도달하지 못했다면 dwaComputeVelocityCommands 함수를 사용하여 DWA 로컬 경로 속도 계산      else {       bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);       if (isOk) {         publishGlobalPlan(transformed_plan);       } else {         ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");         std::vector<geometry_msgs::PoseStamped> empty_plan;         publishGlobalPlan(empty_plan);       }       return isOk;     }   } }; Colored by Color Scripter

cs

dwa_planner.cpp

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381

#include <dwa_local_planner/dwa_planner.h> #include <base_local_planner/goal_functions.h> #include <cmath>   //for computing path distance #include <queue>   #include <angles/angles.h>   #include <ros/ros.h> #include <tf2/utils.h> #include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/point_cloud2_iterator.h>   namespace dwa_local_planner {   void DWAPlanner::reconfigure(DWAPlannerConfig &config)   {       boost::mutex::scoped_lock l(configuration_mutex_);       generator_.setParameters(         config.sim_time,         config.sim_granularity,         config.angular_sim_granularity,         config.use_dwa,         sim_period_);       double resolution = planner_util_->getCostmap()->getResolution();     path_distance_bias_ = resolution * config.path_distance_bias;     // pdistscale used for both path and alignment,      set  forward_point_distance to zero to discard alignment     path_costs_.setScale(path_distance_bias_);     alignment_costs_.setScale(path_distance_bias_);       goal_distance_bias_ = resolution * config.goal_distance_bias;     goal_costs_.setScale(goal_distance_bias_);     goal_front_costs_.setScale(goal_distance_bias_);       occdist_scale_ = config.occdist_scale;     obstacle_costs_.setScale(occdist_scale_);       stop_time_buffer_ = config.stop_time_buffer;     oscillation_costs_.setOscillatixxxxonResetDist     (config.oscillation_reset_dist, config.oscillation_reset_angle);     forward_point_distance_ = config.forward_point_distance;     goal_front_costs_.setXShift(forward_point_distance_);     alignment_costs_.setXShift(forward_point_distance_);       // obstacle costs can vary due to scaling footprint feature     obstacle_costs_.setParams(config.max_vel_trans,      config.max_scaling_factor, config.scaling_speed);       twirling_costs_.setScale(config.twirling_scale);       int vx_samp, vy_samp, vth_samp;     vx_samp = config.vx_samples;     vy_samp = config.vy_samples;     vth_samp = config.vth_samples;       if (vx_samp <= 0) {       ROS_WARN("You've specified that you don't want any samples in the x dimension.        We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");       vx_samp = 1;       config.vx_samples = vx_samp;     }       if (vy_samp <= 0) {       ROS_WARN("You've specified that you don't want any samples in the y dimension.        We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");       vy_samp = 1;       config.vy_samples = vy_samp;     }       if (vth_samp <= 0) {       ROS_WARN("You've specified that you don't want any samples in the th dimension.        We'll at least assume that you want to sample one value... so we're going to set vth_samples to 1 instead");       vth_samp = 1;       config.vth_samples = vth_samp;     }       vsamples_[0] = vx_samp;     vsamples_[1] = vy_samp;     vsamples_[2] = vth_samp;       }     DWAPlanner::DWAPlanner(std::string name, base_local_planner::LocalPlannerUtil *planner_util) :       planner_util_(planner_util),       obstacle_costs_(planner_util->getCostmap()),       path_costs_(planner_util->getCostmap()),       goal_costs_(planner_util->getCostmap(), 0.0, 0.0, true),       goal_front_costs_(planner_util->getCostmap(), 0.0, 0.0, true),       alignment_costs_(planner_util->getCostmap())   {     ros::NodeHandle private_nh("~/" + name);       goal_front_costs_.setStopOnFailure( false );     alignment_costs_.setStopOnFailure( false );       //Assuming this planner is being run within the navigation stack, we can     //just do an upward search for the frequency at which its being run. This     //also allows the frequency to be overwritten locally.     std::string controller_frequency_param_name;     if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {       sim_period_ = 0.05;     } else {       double controller_frequency = 0;       private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);       if(controller_frequency > 0) {         sim_period_ = 1.0 / controller_frequency;       } else {         ROS_WARN("A controller_frequency less than 0 has been set.          Ignoring the parameter, assuming a rate of 20Hz");         sim_period_ = 0.05;       }     }     ROS_INFO("Sim period is set to %.2f", sim_period_);       oscillation_costs_.resetOscillationFlags();       bool sum_scores;     private_nh.param("sum_scores", sum_scores, false);     obstacle_costs_.setSumScores(sum_scores);         private_nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);     map_viz_.initialize(name, planner_util->getGlobalFrame(),      boost::bind(&DWAPlanner::getCellCosts, this, _1, _2, _3, _4, _5, _6));       private_nh.param("global_frame_id", frame_id_, std::string("odom"));       traj_cloud_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("trajectory_cloud", 1);     private_nh.param("publish_traj_pc", publish_traj_pc_, false);       // set up all the cost functions that will be applied in order     // (any function returning negative values will abort scoring, so the order can improve performance)     std::vector<base_local_planner::TrajectoryCostFunction*> critics;     critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)     critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles     critics.push_back(&goal_front_costs_); // prefers trajectories that make the nose go towards (local) nose goal     critics.push_back(&alignment_costs_); // prefers trajectories that keep the robot nose on nose path     critics.push_back(&path_costs_); // prefers trajectories on global path     critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation     critics.push_back(&twirling_costs_); // optionally prefer trajectories that don't spin       // trajectory generators     std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;     generator_list.push_back(&generator_);       scored_sampling_planner_      = base_local_planner::SimpleScoredSamplingPlanner(generator_list, critics);       private_nh.param("cheat_factor", cheat_factor_, 1.0);   }     // used for visualization only, total_costs are not really total costs   bool DWAPlanner::getCellCosts(int cx, int cy, float &path_cost,    float &goal_cost, float &occ_cost, float &total_cost) {       path_cost = path_costs_.getCellCosts(cx, cy);     goal_cost = goal_costs_.getCellCosts(cx, cy);     occ_cost = planner_util_->getCostmap()->getCost(cx, cy);     if (path_cost == path_costs_.obstacleCosts() ||         path_cost == path_costs_.unreachableCellCosts() ||         occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {       return false;     }       total_cost =         path_distance_bias_ * path_cost +         goal_distance_bias_ * goal_cost +         occdist_scale_ * occ_cost;     return true;   }     bool DWAPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {     oscillation_costs_.resetOscillationFlags();     return planner_util_->setPlan(orig_global_plan);   }      //위치/속도를 통해 궤도가 올바른지 확인   //pos : 로봇의 위치   //vel : 로봇의 속도   //vel_samples : 원하는 속도   //return : 궤적이 유효하면 참, 그렇지 않으면 거짓   bool DWAPlanner::checkTrajectory(       Eigen::Vector3f pos,       Eigen::Vector3f vel,       Eigen::Vector3f vel_samples){     oscillation_costs_.resetOscillationFlags();     base_local_planner::Trajectory traj;     geometry_msgs::PoseStamped goal_pose = global_plan_.back();     Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y,      tf2::getYaw(goal_pose.pose.orientation));     base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();     generator_.initialise(pos,         vel,         goal,         &limits,         vsamples_);     generator_.generateTrajectory(pos, vel, vel_samples, traj);     double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);     //if the trajectory is a legal one... the check passes     if(cost >= 0) {       return true;     }     ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vel_samples[0], vel_samples[1], vel_samples[2], cost);       //otherwise the check fails     return false;   }        //계획하기 전 전역 경로가 로컬 맵에 매핑   //global_pose : 로봇의 현재 포즈   //new_plan : 새로운 글로벌 플랜   //Footprint_spec : 로봇의 footprint   void DWAPlanner::updatePlanAndLocalCosts(       const geometry_msgs::PoseStamped& global_pose,       const std::vector<geometry_msgs::PoseStamped>& new_plan,       const std::vector<geometry_msgs::Point>& footprint_spec) {     global_plan_.resize(new_plan.size());     for (unsigned int i = 0; i < new_plan.size(); ++i) {       global_plan_[i] = new_plan[i];     }       obstacle_costs_.setFootprint(footprint_spec);       // 경로를 벗어나는 비용     path_costs_.setTargetPoses(global_plan_);       // costs for not going towards the local goal as much as possible     goal_costs_.setTargetPoses(global_plan_);       // alignment costs     geometry_msgs::PoseStamped goal_pose = global_plan_.back();       Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y,      tf2::getYaw(global_pose.pose.orientation));     double sq_dist =         (pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +         (pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);       // we want the robot nose to be drawn to its final position     // (before robot turns towards goal orientation), not the end of the     // path for the robot center. Choosing the final position after     // turning towards goal orientation causes instability when the     // robot needs to make a 180 degree turn at the end     std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;     double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);     front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +       forward_point_distance_ * cos(angle_to_goal);     front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *       sin(angle_to_goal);       goal_front_costs_.setTargetPoses(front_global_plan);          // keeping the nose on the path     if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {       alignment_costs_.setScale(path_distance_bias_);       // costs for robot being aligned with path (nose on path, not ju       alignment_costs_.setTargetPoses(global_plan_);     } else {       // once we are close to goal, trying to keep the nose close to anything destabilizes behavior.       alignment_costs_.setScale(0.0);     }   }       //로봇의 현재 위치와 속도가 주어지면 실행할 최적의 궤적을 찾습니다.   //global_pose : 로봇의 현재 위치   //global_vel : 로봇의 현재 속도   //drive_velocities : 로봇에 보낼 속도   //return : 최적의 local 경로   base_local_planner::Trajectory DWAPlanner::findBestPath(       const geometry_msgs::PoseStamped& global_pose,       const geometry_msgs::PoseStamped& global_vel,       geometry_msgs::PoseStamped& drive_velocities) {       //make sure that our configuration doesn't change mid-run     boost::mutex::scoped_lock l(configuration_mutex_);       //속도 등 초기화     Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y,      tf2::getYaw(global_pose.pose.orientation));     Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y,      tf2::getYaw(global_vel.pose.orientation));     geometry_msgs::PoseStamped goal_pose = global_plan_.back();     Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y,      tf2::getYaw(goal_pose.pose.orientation));     base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();       // 초기화     generator_.initialise(pos,         vel,         goal,         &limits,         vsamples_);       result_traj_.cost_ = -7;     //경로를 생성하여 각각 경로의 점수를 매기고 최적의 경로 선택     std::vector<base_local_planner::Trajectory> all_explored;     scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);       if(publish_traj_pc_)     {         sensor_msgs::PointCloud2 traj_cloud;         traj_cloud.header.frame_id = frame_id_;         traj_cloud.header.stamp = ros::Time::now();           sensor_msgs::PointCloud2Modifier cloud_mod(traj_cloud);         cloud_mod.setPointCloud2Fields(5, "x", 1, sensor_msgs::PointField::FLOAT32,                                           "y", 1, sensor_msgs::PointField::FLOAT32,                                           "z", 1, sensor_msgs::PointField::FLOAT32,                                           "theta", 1, sensor_msgs::PointField::FLOAT32,                                           "cost", 1, sensor_msgs::PointField::FLOAT32);           unsigned int num_points = 0;         for(std::vector<base_local_planner::Trajectory>::iterator t         =all_explored.begin(); t != all_explored.end(); ++t)         {             if (t->cost_<0)               continue;             num_points += t->getPointsSize();         }           cloud_mod.resize(num_points);         sensor_msgs::PointCloud2Iterator<float> iter_x(traj_cloud, "x");         for(std::vector<base_local_planner::Trajectory>::iterator t         =all_explored.begin(); t != all_explored.end(); ++t)         {             if(t->cost_<0)                 continue;             // Fill out the plan             for(unsigned int i = 0; i < t->getPointsSize(); ++i) {                 double p_x, p_y, p_th;                 t->getPoint(i, p_x, p_y, p_th);                 iter_x[0] = p_x;                 iter_x[1] = p_y;                 iter_x[2] = 0.0;                 iter_x[3] = p_th;                 iter_x[4] = t->cost_;                 ++iter_x;             }         }         traj_cloud_pub_.publish(traj_cloud);     }       // verbose publishing of point clouds     if (publish_cost_grid_pc_) {       //we'll publish the visualization of the costs to rviz before returning our best trajectory       map_viz_.publishCostCloud(planner_util_->getCostmap());     }       // debrief stateful scoring functions     oscillation_costs_.updateOscillationFlags(pos, &result_traj_,      planner_util_->getCurrentLimits().min_vel_trans);       //if we don't have a legal trajectory, we'll just command zero     if (result_traj_.cost_ < 0) {       drive_velocities.pose.position.x = 0;       drive_velocities.pose.position.y = 0;       drive_velocities.pose.position.z = 0;       drive_velocities.pose.orientation.w = 1;       drive_velocities.pose.orientation.x = 0;       drive_velocities.pose.orientation.y = 0;       drive_velocities.pose.orientation.z = 0;     } else {       drive_velocities.pose.position.x = result_traj_.xv_;       drive_velocities.pose.position.y = result_traj_.yv_;       drive_velocities.pose.position.z = 0;       tf2::Quaternion q;       q.setRPY(0, 0, result_traj_.thetav_);       tf2::convert(q, drive_velocities.pose.orientation);     }       return result_traj_;   } };   Colored by Color Scripter

cs