diff --git a/.gitignore b/.gitignore index 53ef6c4..bf650b6 100644 --- a/.gitignore +++ b/.gitignore @@ -16,3 +16,10 @@ build # Version control .svn + +.vscode/ + +.idea/ + +# Compiled files +mpc_local_planner/cmake-build-debug/ diff --git a/mpc_local_planner/src/mpc_local_planner_ros.cpp b/mpc_local_planner/src/mpc_local_planner_ros.cpp index a258bb5..fc65870 100644 --- a/mpc_local_planner/src/mpc_local_planner_ros.cpp +++ b/mpc_local_planner/src/mpc_local_planner_ros.cpp @@ -143,13 +143,14 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm nh.param("footprint_model/is_footprint_dynamic", _params.is_footprint_dynamic, _params.is_footprint_dynamic); - nh.param("collision_avoidance/include_costmap_obstacles", _params.include_costmap_obstacles, _params.include_costmap_obstacles); - nh.param("collision_avoidance/costmap_obstacles_behind_robot_dist", _params.costmap_obstacles_behind_robot_dist, + nh.param("collision/include_costmap_obstacles", _params.include_costmap_obstacles, _params.include_costmap_obstacles); + nh.param("collision/costmap_obstacles_behind_robot_dist", _params.costmap_obstacles_behind_robot_dist, _params.costmap_obstacles_behind_robot_dist); - nh.param("collision_avoidance/collision_check_no_poses", _params.collision_check_no_poses, _params.collision_check_no_poses); - nh.param("collision_avoidance/collision_check_min_resolution_angular", _params.collision_check_min_resolution_angular, + nh.param("collision/collision_check_no_poses", _params.collision_check_no_poses, _params.collision_check_no_poses); + nh.param("collision/collision_check_min_resolution_angular", _params.collision_check_min_resolution_angular, _params.collision_check_min_resolution_angular); + ROS_INFO("collision/collision_check_no_poses:%d",_params.collision_check_no_poses); // 代价地图转换器插件相关参数 nh.param("costmap_converter_plugin", _costmap_conv_params.costmap_converter_plugin, _costmap_conv_params.costmap_converter_plugin); @@ -353,7 +354,7 @@ uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt // 修剪全局计划,剪掉机器人之前的部分 pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance); - // 修剪全局计划,剪掉机器人之前的部分 + // Convert To Local frame std::vector transformed_plan; int goal_idx; geometry_msgs::TransformStamped tf_plan_to_global;