1 / 44

ROS - Lesson 6

ROS - Lesson 6. Teaching Assistant: Roi Yehoshua roiyeho@gmail.com. Agenda. Navigation planners Adaptive Monte-Carlo Localization. Navigation Stack. Global Planner. http://wiki.ros.org/navfn A package that provides functions to create the plans

adolph
Download Presentation

ROS - Lesson 6

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. ROS - Lesson 6 Teaching Assistant: RoiYehoshua roiyeho@gmail.com

  2. Agenda • Navigation planners • Adaptive Monte-Carlo Localization (C)2013 Roi Yehoshua

  3. Navigation Stack (C)2013 Roi Yehoshua

  4. Global Planner • http://wiki.ros.org/navfn • A package that provides functions to create the plans • The global plan is computed before the robot starts moving toward the next destination • The planner assumes a circular robot and operates on a costmap to find a minimum cost plan from a start point to an end point in a grid. • The navigation function is computed using Dijkstra's algorithm • support for an A* heuristic may also be added in the near future (C)2013 Roi Yehoshua

  5. Navfn Published Topics • The last plan computed by navfn is published on the topic /move_base_node/NavfnROS/plan everytime the planner computes a new path • This is used primarily for visualization purposes • Message type is nav_msgs/Path (C)2013 Roi Yehoshua

  6. Navfn Published Topics (C)2013 Roi Yehoshua

  7. NavfnROS • The navfn::NavfnROS object is a wrapper for a navfn::NavFn object that exposes its functionality as a C++ ROS Wrapper. • It adheres to the nav_core::BaseGlobalPlannerinterface found in the nav_core package. (C)2013 Roi Yehoshua

  8. Base Local Planner • http://wiki.ros.org/base_local_planner • The local planner monitors incoming sensor data and chooses appropriate linear and angular velocities for the robot to traverse the current segment of the global path. • The base_local_planner combines odometry data with both global and local cost maps to select a path for the robot to follow • The base local planner can recompute the robot's path on the fly to keep the robot from striking objects yet still allowing it to reach its destination. (C)2013 Roi Yehoshua

  9. Base Local Planner • The base local planner implements the Trajectory Rollout and Dynamic Window algorithm • References: • Brian P. Gerkey and Kurt Konolige. "Planning and Control in Unstructured Terrain". Discussion of the Trajectory Rollout algorithm in use on the LAGR robot. • D. Fox, W. Burgard, and S. Thrun. "The dynamic window approach to collision avoidance". The Dynamic Window Approach to local control. (C)2013 Roi Yehoshua

  10. Trajectory Rollout Algorithm • Discretely sample in the robot's control space (dx,dy,dθ) • For each sampled velocity, perform forward simulation from the robot's current state to predict what would happen if the sampled velocity were applied for some (short) period of time. • Evaluate each trajectory resulting from the forward simulation, using a metric that incorporates characteristics such as: proximity to obstacles, proximity to the goal, proximity to the global path, and speed. • Discard illegal trajectories (those that collide with obstacles). • Pick the highest-scoring trajectory and send the associated velocity to the mobile base. • Rinse and repeat. (C)2013 Roi Yehoshua

  11. Trajectory Rollout Algorithm Taken from ROS Wiki (C)2013 Roi Yehoshua

  12. DWA • DWA (Dynamic Window Algorithm) differs from Trajectory Rollout in how the robot's control space is sampled. • Trajectory Rollout samples from the set of achievable velocities over the entire forward simulation period given the acceleration limits of the robot, while DWA samples from the set of achievable velocities for just one simulation step given the acceleration limits of the robot. • DWA is a more efficient algorithm because it samples a smaller space, but may be outperformed by Trajectory Rollout for robots with low acceleration limits because DWA does not forward simulate constant accelerations. • In practice, DWA and Trajectory Rollout perform comparably and thus it is recommend to use of DWA for its efficiency gains. (C)2013 Roi Yehoshua

  13. Map Grid • In order to score trajectories efficiently, a Map Grid is used. • For each control cycle, a grid is created around the robot (the size of the local costmap), and the global path is mapped onto this area. • This means certain of the grid cells will be marked with distance 0 to a path point, and distance 0 to the goal. • A propagation algorithm then efficiently marks all other cells with their Manhattan distance to the closest of the points marked with zero. (C)2013 Roi Yehoshua

  14. Oscillation Suppression • Oscillation occur when in either of the x, y, or theta dimensions, positive and negative values are chosen consecutively. • To prevent oscillations, when the robot moves in any direction, for the next cycles the opposite direction is marked invalid, until the robot has moved beyond a certain distance from the position where the flag was set. (C)2013 Roi Yehoshua

  15. Base Local Planner Published Topics • global_plan • The portion of the global plan that the local planner is currently attempting to follow • Message Type: nav_msgs/Path • local_plan • The local plan or trajectory that scored the highest on the last cycle • Message Type: nav_msgs/Path • cost_cloud • The cost grid used for planning • Message Type: sensor_msgs/PointCloud2 (C)2013 Roi Yehoshua

  16. Base Local Planner Parameters • There are a large number of ROS Parameters that can be set to customize the behavior of the base_local_planner::TrajectoryPlannerROS wrapper. • These parameters are grouped into several categories: • robot configuration • goal tolerance • forward simulation • trajectory scoring • oscillation prevention • global plan (C)2013 Roi Yehoshua

  17. Robot Configuration Parameters (1) (C)2013 Roi Yehoshua

  18. Robot Configuration Parameters (2) (C)2013 Roi Yehoshua

  19. Goal Tolerance Parameters (C)2013 Roi Yehoshua

  20. Forward Simulation Parameters (C)2013 Roi Yehoshua

  21. Trajectory Scoring Parameters • The cost function used to score each trajectory is in the following form: (C)2013 Roi Yehoshua

  22. Trajectory Scoring Parameters (C)2013 Roi Yehoshua

  23. base_local_planner.yaml (1) • #For full documentation of the parameters in this file, and a list of all the • #parameters available for TrajectoryPlannerROS, please see • #http://www.ros.org/wiki/base_local_planner • TrajectoryPlannerROS: • #Set the acceleration limits of the robot • acc_lim_th: 3.2 • acc_lim_x: 2.5 • acc_lim_y: 2.5 • #Set the velocity limits of the robot • max_vel_x: 0.65 • min_vel_x: 0.1 • max_rotational_vel: 1.0 • min_in_place_rotational_vel: 0.4 • #The velocity the robot will command when trying to escape from a stuck situation • escape_vel: -0.1 • #For this example, we'll use a holonomic robot • holonomic_robot: true • #Since we're using a holonomic robot, we'll set the set of y velocities it will sample • y_vels: [-0.3, -0.1, 0.1, -0.3] (C)2013 Roi Yehoshua

  24. base_local_planner.yaml (2) • #Set the tolerance on achieving a goal • xy_goal_tolerance: 0.1 • yaw_goal_tolerance: 0.05 • #We'll configure how long and with what granularity we'll forward simulate trajectories • sim_time: 1.7 • sim_granularity: 0.025 • vx_samples: 3 • vtheta_samples: 20 • #Parameters for scoring trajectories • goal_distance_bias: 0.8 • path_distance_bias: 0.6 • occdist_scale: 0.01 • heading_lookahead: 0.325 • #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example • dwa: true • #How far the robot must travel before oscillation flags are reset • oscillation_reset_dist: 0.05 • #Eat up the plan as the robot moves along it • prune_plan: true (C)2013 Roi Yehoshua

  25. TrajectoryPlannerROS • The base_local_planner::TrajectoryPlannerROS object is a wrapper for a base_local_planner::TrajectoryPlanner object that exposes its functionality as a ROS Wrapper. • It adheres to the nav_core::BaseLocalPlannerinterface found in the nav_core package. (C)2013 Roi Yehoshua

  26. TrajectoryPlannerROS (C)2013 Roi Yehoshua

  27. Navigation Plans in rviz • NavFn Plan • Displays the full plan for the robot computed by the global planner • Topic: /move_base_node/NavfnROS/plan • Global Plan • It shows the portion of the global plan that the local planner is currently pursuing. • Topic: /move_base_node/TrajectoryPlannerROS/global_plan • Local Plan • It shows the trajectory associated with the velocity commands currently being commanded to the base by the local planner • Topic: /move_base_node/TrajectoryPlannerROS/local_plan (C)2013 Roi Yehoshua

  28. Navigation Plans in rviz NavFn Plan Local Plan Global Plan (C)2013 Roi Yehoshua

  29. Changing Trajectory Scoring • Run the rqt_reconfigure tool • This tool allows changing dynamic configuration values • Open the move_base group • Select the TrajectoryPlannerROS node • Then set the pdist_scale parameter to something high like 2.5 • After that, you should see that the local path (blue) now more closely follows the global path (yellow). • $ rosrunrqt_reconfigurerqt_reconfigure (C)2013 Roi Yehoshua

  30. Changing Trajectory Scoring (C)2013 Roi Yehoshua

  31. Changing Trajectory Scoring Local plan follows global plan (C)2013 Roi Yehoshua

  32. Localization (C)2013 Roi Yehoshua

  33. Localization • Localization is the problem of estimating the pose of the robot relative to a map • Localization is not terribly sensitive to the exact placement of objects so it can handle small changes to the locations of objects • ROS uses the amcl package for localization (C)2013 Roi Yehoshua

  34. AMCL • amcl is a probabilistic localization system for a robot moving in 2D. • It implements the adaptive Monte Carlo localization approach, which uses a particle filter to track the pose of a robot against a known map. • The algorithm and its parameters are described in the book Probabilistic Robotics by Thrun, Burgard, and Fox. • http://www.probabilistic-robotics.org/ • As currently implemented, this node works only with laser scans and laser maps. It could be extended to work with other sensor data. (C)2013 Roi Yehoshua

  35. AMCL • amcl takes in a laser-based map, laser scans, and transform messages, and outputs pose estimates • Subscribed topics: • scan – Laser scans • tf – Transforms • initialpose – Mean and covariance with which to (re-) initialize the particle filter • map – the map used for laser-based localization • Published topics: • amcl_pose – Robot's estimated pose in the map, with covariance. • Particlecloud – The set of pose estimates being maintained by the filter (C)2013 Roi Yehoshua

  36. AMCL Parameters (C)2013 Roi Yehoshua

  37. amcl_node.xml (1) • <launch> • <!-- • Example amcl configuration. Descriptions of parameters, as well as a full list of all amcl parameters, can be found at http://www.ros.org/wiki/amcl. • --> • <node pkg="amcl" type="amcl" name="amcl" respawn="true"> • <remap from="scan" to="base_scan" /> • <!-- Publish scans from best pose at a max of 10 Hz --> • <param name="odom_model_type" value="omni"/> • <param name="odom_alpha5" value="0.1"/> • <param name="transform_tolerance" value="0.2" /> • <param name="gui_publish_rate" value="10.0"/> • <param name="laser_max_beams" value="30"/> • <param name="min_particles" value="500"/> • <param name="max_particles" value="5000"/> • <param name="kld_err" value="0.05"/> • <param name="kld_z" value="0.99"/> • <param name="odom_alpha1" value="0.2"/> • <param name="odom_alpha2" value="0.2"/> (C)2013 Roi Yehoshua

  38. amcl_node.xml (2) • <!-- translation std dev, m --> • <param name="odom_alpha3" value="0.8"/> • <param name="odom_alpha4" value="0.2"/> • <param name="laser_z_hit" value="0.5"/> • <param name="laser_z_short" value="0.05"/> • <param name="laser_z_max" value="0.05"/> • <param name="laser_z_rand" value="0.5"/> • <param name="laser_sigma_hit" value="0.2"/> • <param name="laser_lambda_short" value="0.1"/> • <param name="laser_lambda_short" value="0.1"/> • <param name="laser_model_type" value="likelihood_field"/> • <!-- <param name="laser_model_type" value="beam"/> --> • <param name="laser_likelihood_max_dist" value="2.0"/> • <param name="update_min_d" value="0.2"/> • <param name="update_min_a" value="0.5"/> • <param name="odom_frame_id" value="odom"/> • <param name="resample_interval" value="1"/> • <param name="transform_tolerance" value="0.1"/> • <param name="recovery_alpha_slow" value="0.0"/> • <param name="recovery_alpha_fast" value="0.0"/> • </node> • </launch> (C)2013 Roi Yehoshua

  39. Particle Cloud in rviz • The Particle Cloud displays shows the particle cloud used by the robot's localization system. • The spread of the cloud represents the localization system's uncertainty about the robot's pose. • A cloud that spreads out a lot reflects high uncertainty, while a condensed cloud represents low uncertainty • As the robot moves about the environment, this cloud should shrink in size as additional scan data allows amcl to refine its estimate of the robot's position and orientation (C)2013 Roi Yehoshua

  40. Particle Cloud in rviz • To watch the particle cloud in rviz: • Click Add Display and choose Pose Array • Set topic name to /particlecloud (C)2013 Roi Yehoshua

  41. Particle Cloud in rviz (C)2013 Roi Yehoshua

  42. Using amcl with a Real Robot Taken from ROS by Example / Goebel (C)2013 Roi Yehoshua

  43. Fake Localization • http://wiki.ros.org/fake_localization • A ROS node that simply forwards odometry information. • Substitutes for a localization system • Converts odometry data into pose, particle cloud, and transform data of the form published by amcl. • Mostly used during simulation as a method to provide perfect localization in a computationally inexpensive manner. (C)2013 Roi Yehoshua

  44. Navigation Summary (C)2013 Roi Yehoshua

More Related