本篇給予實際項目，作一個總結歸納
部分參考自
（１）．http://blog.csdn.net/u013158492/article/details/50493676
（２）. http://blog.csdn.net/heyijia0327/article/details/44983551
1.wiki
主要用到的包：
leg_detector
輸入：takes sensor_msgs/LaserScans as input and uses a machine-learning-trained classifier to detect groups of laser readings as possible legs
輸出： publish people_msgs/PositionMeasurementArrays for the individual legs, and it can also attempt to pair the legs together and publish their average as an estimate of where the center of one person is as apeople_msgs/PositionMeasurement.
訂閱話題：
scan (sensor_msgs/LaserScan)
- The laser scan to detect from.
- 注意：在unseeded模式下，即只有一個激光傳感器模式下，無其他檢測源可以修正leg_detector檢測到人腿的對與錯，因此可能存在大量誤檢，但適用場景廣
people_tracker_filter (people_msgs/PositionMeasurement)
- PositionMeasurements to use as the seeds (see above)
- 在seeded模式下，有其他傳感器源，可提高行人檢測准確性。一般來說，可加入攝像頭，提供的message為people_msgs/PositionMeasurement
發布話題：
leg_tracker_measurements (people_msgs/PositionMeasurementArray)
- Estimates of where legs are
people_tracker_measurements (people_msgs/PositionMeasurementArray)
- Estimates of where people are
visualization_marker (visualization_msgs/Marker)
- Markers where legs and people are
訂閱話題：
people_tracker_measurements (people_msgs/PositionMeasurementArray)
- Positions of people
發布話題：
/people (people_msgs/People)
- Positions and velocities of people
/visualization_marker (visualization_msgs/Marker)
- Markers
2. sending two types of sensor streams, sensor_msgs/LaserScan messages and sensor_msgs/PointCloud messages over ROS
http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors
構造函數
ObstacleLayer() { costmap_ = NULL;}// this is the unsigned char* member of parent class Costmap2D.這里指明了costmap_指針保存了Obstacle這一層的地圖數據
對於ObstacleLater，首先分析其需要實現的Layer層的方法：
virtual void onInitialize(); virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y); virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); virtual void activate(); virtual void deactivate(); virtual void reset();
函數 onInitialize();：
首先獲取參數設定的值，然后新建observation buffer：
// create an observation buffer observation_buffers_.push_back(boost::shared_ptr < ObservationBuffer> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,sensor_frame, transform_tolerance))); // check if we'll add this buffer to our marking observation buffers
if (marking) marking_buffers_.push_back(observation_buffers_.back()); // check if we'll also add this buffer to our clearing observation buffers if (clearing) clearing_buffers_.push_back(observation_buffers_.back());
然后分別對不同的sensor類型如LaserScan PointCloud PointCloud2，注冊不同的回調函數。這里選LaserScan 分析其回調函數：
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, const boost::shared_ptr<ObservationBuffer>& buffer) { // project the laser into a point cloud sensor_msgs::PointCloud2 cloud; cloud.header = message->header; // project the scan into a point cloud try { projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_); } catch (tf::TransformException &ex) { ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), ex.what()); projector_.projectLaser(*message, cloud); } // buffer the point cloud buffer->lock(); buffer->bufferCloud(cloud); buffer->unlock(); }
其中buffer->bufferCloud(cloud); 實際上是sensor_msgs::PointCloud2 >>>pcl::PCLPointCloud2 >>> pcl::PointCloud < pcl::PointXYZ > ; 然后才調用void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud) ：
void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud) { Stamped < tf::Vector3 > global_origin; // create a new observation on the list to be populated observation_list_.push_front(Observation()); // check whether the origin frame has been set explicitly or whether we should get it from the cloud string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; try { // given these observations come from sensors... we'll need to store the origin pt of the sensor Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0), pcl_conversions::fromPCL(cloud.header).stamp, origin_frame); tf_.waitForTransform(global_frame_, local_origin.frame_id_, local_origin.stamp_, ros::Duration(0.5)); tf_.transformPoint(global_frame_, local_origin, global_origin); observation_list_.front().origin_.x = global_origin.getX(); observation_list_.front().origin_.y = global_origin.getY(); observation_list_.front().origin_.z = global_origin.getZ(); // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations observation_list_.front().raytrace_range_ = raytrace_range_; observation_list_.front().obstacle_range_ = obstacle_range_; pcl::PointCloud < pcl::PointXYZ > global_frame_cloud; // transform the point cloud pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_); global_frame_cloud.header.stamp = cloud.header.stamp; //上面的操作都是針對 observation_list_.front()的一些meta數據作賦值,下面的操作是對(observation_list_.front().cloud_)作賦值操作， // now we need to remove observations from the cloud that are below or above our height thresholds pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_); unsigned int cloud_size = global_frame_cloud.points.size(); observation_cloud.points.resize(cloud_size); unsigned int point_count = 0; // copy over the points that are within our height bounds for (unsigned int i = 0; i < cloud_size; ++i) { if (global_frame_cloud.points[i].z <= max_obstacle_height_ && global_frame_cloud.points[i].z >= min_obstacle_height_) { observation_cloud.points[point_count++] = global_frame_cloud.points[i]; } } // resize the cloud for the number of legal points observation_cloud.points.resize(point_count); observation_cloud.header.stamp = cloud.header.stamp; observation_cloud.header.frame_id = global_frame_cloud.header.frame_id; } catch (TransformException& ex) { // if an exception occurs, we need to remove the empty observation from the list observation_list_.pop_front(); ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); return; } // if the update was successful, we want to update the last updated time last_updated_ = ros::Time::now(); // we'll also remove any stale observations from the list //這個操作會將timestamp較早的點都移除出observation_list_ purgeStaleObservations(); }
以下重點分析updateBounds：
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,double* min_y, double* max_x, double* max_y) { if (rolling_window_) updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); if (!enabled_) return; useExtraBounds(min_x, min_y, max_x, max_y); bool current = true; std::vector<Observation> observations, clearing_observations; // get the marking observations current = current && getMarkingObservations(observations); // get the clearing observations current = current &&getClearingObservations(clearing_observations); // update the global current status current_ = current; // raytrace freespace for (unsigned int i = 0; i < clearing_observations.size(); ++i) { raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);//首先清理出傳感器到被測物之間的區域，標記為FREE_SPACE } // place the new obstacles into a priority queue... each with a priority of zero to begin with for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it) { const Observation& obs = *it; const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_); double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; for (unsigned int i = 0; i < cloud.points.size(); ++i) { double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z; // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { ROS_DEBUG("The point is too high"); continue; } // compute the squared distance from the hitpoint to the pointcloud's origin double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + (pz - obs.origin_.z) * (pz - obs.origin_.z); // if the point is far enough away... we won't consider it if (sq_dist >= sq_obstacle_range) { ROS_DEBUG("The point is too far away"); continue; } // now we need to compute the map coordinates for the observation unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { ROS_DEBUG("Computing map coords failed"); continue; } unsigned int index = getIndex(mx, my); costmap_[index] = LETHAL_OBSTACLE; touch(px, py, min_x, min_y, max_x, max_y); } } updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); }
函數raytraceFreespace：
會首先處理測量值越界的問題，然后調用:
MarkCell marker(costmap_, FREE_SPACE); // and finally... we can execute our trace to clear obstacles along that line raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
最終raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 會將所有在(x0,y0)>>(x1,y1)之間的所有cell標記為FREE_SPACE。而updateRaytraceBounds 會根據測量的距離，更新擴張（min_x, min_y, max_x, max_y）。
updateBounds 在根據測量數據完成clear 操作之后，就開始了mark 操作，對每個測量到的點，標記為obstacle
double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z; // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { ROS_DEBUG("The point is too high"); continue; } // compute the squared distance from the hitpoint to the pointcloud's origin double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + (pz - obs.origin_.z) * (pz - obs.origin_.z); // if the point is far enough away... we won't consider it if (sq_dist >= sq_obstacle_range) { ROS_DEBUG("The point is too far away"); continue; } // now we need to compute the map coordinates for the observation unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { ROS_DEBUG("Computing map coords failed"); continue; } unsigned int index = getIndex(mx, my); costmap_[index] = LETHAL_OBSTACLE; touch(px, py, min_x, min_y, max_x, max_y); }
函數 updateFootprint：
void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) { if (!footprint_clearing_enabled_) return; transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);//這里獲得了在當前機器人位姿（robot_x, robot_y, robot_yaw）條件下，機器人輪廓點在global坐標系下的值 for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);//再次保留或者擴張Bounds } }
函數 updateCosts：
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; if (footprint_clearing_enabled_) { setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);//設置機器人輪廓所在區域為FREE_SPACE } switch (combination_method_) { case 0: // Overwrite調用的CostmapLayer提供的方法 updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; case 1: // Maximum updateWithMax(master_grid, min_i, min_j, max_i, max_j); break; default: // Nothing break; } }
本文分析move base 的配置文件，從配置文件設置的角度，可以更清晰的把握move base對於costmap2D，global planner，local planner的調用關系。
這里采用turtlebot_navigation package 為例：
<launch> <include file="$(find turtlebot_navigation)/launch/includes/velocity_smoother.launch.xml"/> <include file="$(find turtlebot_navigation)/launch/includes/safety_controller.launch.xml"/> <arg name="odom_frame_id" default="odom"/> <arg name="base_frame_id" default="base_footprint"/> <arg name="global_frame_id" default="map"/> <arg name="odom_topic" default="odom" /> <arg name="laser_topic" default="scan" /> <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/dummy.yaml"/> <!-- 以下部分是move base調用，需要配置的文件：包括全局地圖，局部地圖，全局planner，局部planner --> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find turtlebot_navigation)/param/local_costmap_params.yaml" command="load" /> <rosparam file="$(find turtlebot_navigation)/param/global_costmap_params.yaml" command="load" /> <rosparam file="$(find turtlebot_navigation)/param/dwa_local_planner_params.yaml" command="load" /> <rosparam file="$(find turtlebot_navigation)/param/move_base_params.yaml" command="load" /> <rosparam file="$(find turtlebot_navigation)/param/global_planner_params.yaml" command="load" /> <rosparam file="$(find turtlebot_navigation)/param/navfn_global_planner_params.yaml" command="load" /> <!-- external params file that could be loaded into the move_base namespace --> <rosparam file="$(arg custom_param_file)" command="load" /> <!-- reset frame_id parameters using user input data --> <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/> <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/> <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/> <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/> <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/> <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/> <remap from="odom" to="$(arg odom_topic)"/> <remap from="scan" to="$(arg laser_topic)"/> </node> </launch>
從上面的配置上來可以看到以下內容，這些都是關於地圖的配置：
/param/costmap_common_params.yaml" command="load" ns="global_costmap" /param/costmap_common_params.yaml" command="load" ns="local_costmap" /param/local_costmap_params.yaml" command="load" /param/global_costmap_params.yaml" command="load"
地圖配置，首先是采用了一個costmap_common_params.yaml文件配置了一些公共的參數：
max_obstacle_height: 0.60 # assume something like an arm is mounted on top of the robot # Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation) robot_radius: 0.20 # distance a circular robot should be clear of the obstacle (kobuki: 0.18) # footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular map_type: voxel obstacle_layer: enabled: true max_obstacle_height: 0.6 origin_z: 0.0 z_resolution: 0.2 z_voxels: 2 unknown_threshold: 15 mark_threshold: 0 combination_method: 1 track_unknown_space: true #true needed for disabling global path planning through unknown space obstacle_range: 2.5 raytrace_range: 3.0 origin_z: 0.0 z_resolution: 0.2 z_voxels: 2 publish_voxel_map: false observation_sources: scan bump scan: data_type: LaserScan topic: scan marking: true clearing: true min_obstacle_height: 0.25 max_obstacle_height: 0.35 bump: data_type: PointCloud2 topic: mobile_base/sensors/bumper_pointcloud marking: true clearing: false min_obstacle_height: 0.0 max_obstacle_height: 0.15 # for debugging only, let's you see the entire voxel grid #cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns inflation_layer: enabled: true cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost drops off (default: 10) inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. static_layer: enabled: true
然后分別設定global map 和local map：
global_costmap: global_frame: /map robot_base_frame: /base_footprint update_frequency: 1.0 publish_frequency: 0.5 static_map: true transform_tolerance: 0.5 <!-- global map引入了以下三層，經融合構成了master map，用於global planner--> plugins: - {name: static_layer, type: "costmap_2d::StaticLayer"} - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
local_costmap: global_frame: odom robot_base_frame: /base_footprint update_frequency: 5.0 publish_frequency: 2.0 static_map: false rolling_window: true width: 4.0 height: 4.0 resolution: 0.05 transform_tolerance: 0.5 ／／local_planner 要求的實時性還是挺高的（特別是你把速度調高的時候），
而local_costmap 所依賴的全局坐標系一般是odom，繪制costmap的時候會反復詢問odom-》base_link 坐標系的
信息，tf數據延遲要是大了會影響costmap，進而導致機器人planner實時性降低，機器人移動遲緩或者撞上障礙物。
所以有個參數transform_tolerance一定要慎重 <!-- local map引入了以下兩層，經融合構成了master map，用於局部planner--> plugins: - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
然后是planner的配置：
/param/move_base_params.yaml" command="load" /param/global_planner_params.yaml" command="load" /param/navfn_global_planner_params.yaml" command="load" /param/dwa_local_planner_params.yaml" command="load"
文件move_base_params.yaml 內容：
shutdown_costmaps: false controller_frequency: 5.0 controller_patience: 3.0 planner_frequency: 1.0 planner_patience: 5.0 oscillation_timeout: 10.0 oscillation_distance: 0.2 # local planner - default is trajectory rollout base_local_planner: "dwa_local_planner/DWAPlannerROS" <!--這里配置了local planner為dwa_local_planner --> <!--這里配置了global planner為navfn/NavfnROS --> base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner
對於global planner，可以采用以下三種實現之一：
"navfn/NavfnROS","global_planner/GlobalPlanner","carrot_planner/CarrotPlanner"
然后來看global planner的配置：
GlobalPlanner: # Also see: http://wiki.ros.org/global_planner old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false use_quadratic: true # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true use_dijkstra: true # Use dijkstra's algorithm. Otherwise, A*, default true use_grid_path: false # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false allow_unknown: true # Allow planner to plan through unknown space, default true #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work planner_window_x: 0.0 # default 0.0 planner_window_y: 0.0 # default 0.0 default_tolerance: 0.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0 publish_scale: 100 # Scale by which the published potential gets multiplied, default 100 planner_costmap_publish_frequency: 0.0 # default 0.0 lethal_cost: 253 # default 253 neutral_cost: 50 # default 50 cost_factor: 3.0 # Factor to multiply each cost from costmap by, default 3.0 publish_potential: true # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true
對於local planner ，有以下兩種選擇：
"trajectory rollout","dwa_local_planner/DWAPlannerROS"
以下配置了DWAPlannerROS ：
DWAPlannerROS: # Robot Configuration Parameters - Kobuki max_vel_x: 0.5 # 0.55 min_vel_x: 0.0 max_vel_y: 0.0 # diff drive robot min_vel_y: 0.0 # diff drive robot max_trans_vel: 0.5 # choose slightly less than the base's capability min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity trans_stopped_vel: 0.1 # Warning! # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities # are non-negligible and small in place rotational velocities will be created. max_rot_vel: 5.0 # choose slightly less than the base's capability min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity rot_stopped_vel: 0.4 acc_lim_x: 1.0 # maximum is theoretically 2.0, but we acc_lim_theta: 2.0 acc_lim_y: 0.0 # diff drive robot # Goal Tolerance Parameters yaw_goal_tolerance: 0.3 # 0.05 xy_goal_tolerance: 0.15 # 0.10 # latch_xy_goal_tolerance: false # Forward Simulation Parameters sim_time: 1.0 # 1.7 vx_samples: 6 # 3 vy_samples: 1 # diff drive robot, there is only one sample vtheta_samples: 20 # 20 # Trajectory Scoring Parameters path_distance_bias: 64.0 # 32.0 - weighting for how much it should stick to the global path plan goal_distance_bias: 24.0 # 24.0 - wighting for how much it should attempt to reach its goal occdist_scale: 0.5 # 0.01 - weighting for how much the controller should avoid obstacles forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed. # Oscillation Prevention Parameters oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags # Debugging publish_traj_pc : true publish_cost_grid_pc: true global_frame_id: odom # Differential-drive robot configuration - necessary? # holonomic_robot: false
５．機器人局部避障的動態窗口法(dynamic window approach)
首先在V_m∩V_d的范圍內采樣速度： allowable_v = generateWindow(robotV, robotModel) allowable_w = generateWindow(robotW, robotModel) 然后根據能否及時剎車剔除不安全的速度： for each v in allowable_v for each w in allowable_w dist = find_dist(v,w,laserscan,robotModel) breakDist = calculateBreakingDistance(v)//剎車距離 if (dist > breakDist) //如果能夠及時剎車，該對速度可接收 如果這組速度可接受，接下來利用評價函數對其評價，找到最優的速度組
來源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html BEGIN DWA(robotPose,robotGoal,robotModel) laserscan = readScanner() allowable_v = generateWindow(robotV, robotModel) allowable_w = generateWindow(robotW, robotModel) for each v in allowable_v for each w in allowable_w dist = find_dist(v,w,laserscan,robotModel) breakDist = calculateBreakingDistance(v) if (dist > breakDist) //can stop in time heading = hDiff(robotPose,goalPose, v,w) //clearance與原論文稍不一樣 clearance = (dist-breakDist)/(dmax - breakDist) cost = costFunction(heading,clearance, abs(desired_v - v)) if (cost > optimal) best_v = v best_w = w optimal = cost set robot trajectory to best_v, best_w END
那么如何得到（v,w）呢？見：航跡推演（Odometry）