MoveBase::MoveBase(tf2_ros::Buffer &tf) : tf_(tf),
as_(NULL),
planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false)
{
// as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
recovery_trigger_ = PLANNING_R;
// get some parameters that will be global to the move base node
std::string local_planner;
// private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("move_base/saveMapping", saveMapping, false);
private_nh.param("savePath", savePath, std::string("/home/duzhong/dzacs/src/dzglobalplanner/mapping.txt"));
private_nh.param("base_local_planner", local_planner, std::string("dwa_local_planner/DWAPlannerROS"));
// private_nh.param("base_local_planner", local_planner, std::string("teb_local_planner/TebLocalPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
private_nh.param("planner_frequency", planner_frequency_, 1.0);
private_nh.param("controller_frequency", controller_frequency_, 3.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default
printf("savePath----move_base-->-=%s\n\n\n\n", savePath.c_str());
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
// set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
// for commanding the base
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0);
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
all_plan_pub_ = private_nh.advertise<nav_msgs::Path>("allplan", 1);
ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
// we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
// they won't get any useful information back about its status, but this is useful for tools
// like nav_view and rviz
ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
// we'll assume the radius of the robot to be consistent with what's specified for the costmaps
private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 0.21);
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
private_nh.param("road_paths_savePath", road_paths_savePath, std::string("/home/duzhong/dzacs/src/resource/road_paths"));
private_nh.param("stop_points_savePath", stop_points_savePath, std::string("/home/duzhong/dzacs/src/resource/stop_points"));
if (saveMapping)
{
printf("move_base--> saveMapping = true\n");
}
else if (!saveMapping)
{
printf("move_base--> saveMapping = false\n");
}
// create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();
// initialize the global planner
/* try
{
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
}
catch (const pluginlib::PluginlibException &ex)
{
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
} */
// create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();
// create a local planner
try
{
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
}
catch (const pluginlib::PluginlibException &ex)
{
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
// Start actively updating costmaps based on sensor data
planner_costmap_ros_->start();
controller_costmap_ros_->start();
// advertise a service for getting a plan
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
// advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
// if we shutdown our costmaps when we're deactivated... we'll do that now
if (shutdown_costmaps_)
{
ROS_DEBUG_NAMED("move_base", "Stopping costmaps initially");
planner_costmap_ros_->stop();
controller_costmap_ros_->stop();
}
// load any user specified recovery behaviors, and if that fails load the defaults
if (!loadRecoveryBehaviors(private_nh))
{
loadDefaultRecoveryBehaviors();
}
// initially, we'll need to make a plan
state_ = PLANNING;
// we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;
foundObstacleFlag = 0;
pursuitSpeed = 0.6;
pubtargetSpeed = 0.6;
recvScanFlag = 0;
saveMappingMsg.data = 0;
road_points_index = 0;
stop_point_signal_msg.data = 0;
sub_lasersScan = nh.subscribe("scan", 1, &MoveBase::callback_lasersScan, this);
sub_saveMapping = nh.subscribe("savemapping", 1, &MoveBase::callback_saveMapping, this);
hgglobalplannerpub = private_nh.advertise<move_base_msgs::hgpathplanner>("hgglobalplanner", 10);
hglocationpub = private_nh.advertise<move_base_msgs::hglocation>("hglocation", 10);
angle_pub = nh.advertise<geometry_msgs::Twist>("pursuitAngle", 1);
vis_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
stop_point_signal = private_nh.advertise<std_msgs::UInt8>("stop_signal",1);
std::vector<std::string> mapFolders = {
road_paths_savePath,
stop_points_savePath};
loadAllMaps(mapFolders);
roadPoints = road_paths[road_points_index];
stopPoints = stop_Points[0];
doplanner();
}分析一下这一段ROS代码的用处,我想添加一个接受其他几个节点话题发布的消息，对订阅的消息进行综合判断，进而指定导航路线。我该怎么做，事自己写一个类还是在这个MoseBase里面写
本文解决C++遗传编程项目中的编译错误与警告，包括零大小数组使用及局部指针变量未初始化等问题，并给出修改建议。
扫一扫
1924
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
