Quantcast
Channel: ROS Answers: Open Source Q&A Forum - RSS feed
Viewing all articles
Browse latest Browse all 667

Move_base/dwa parameters for a large robot with rotation center in the front

$
0
0
Dear Navigation experts and other respective ROS users, I have been struggling with an annoying problem for a while which I hope you can help me to solve it. **Robot properties:** I have a robot with the following size: **70x60x50(L,W,H)**. (Volksbot RT3 differential drive, modified and bigger). This robot has two maxon 150w engines, and two maxon controllers with 74:1 gears. The rotation centre of the robot is on the front axis of the robot, and therefore I have the base_link and base_footprint on that point. This point is 12cm behind from the front of the robot. We use ROS Hydro, Ubuntu 12.04 Sensors: Laser: in the front - Front_rotating_xtion -> down sampled to leaf size of 5cm - Back_fixed_xtion -> down sampled to leaf size of 5cm - I use multiple obstacle layers so the sensors do not overwrite each other Note: We started using xtions/kinect since Fuerte, and we always had to make two observation sources for one camera, and set one for marking and the other for clearing. Otherwise it wouldn't work. (Perhaps it changed but we didn't try) **Problem** When I give a navigation goal, the global plan is somewhat reasonable. I intentionally increased the inflation radius in the global costmap to avoid having tricky situation. My problem is that the dwa local planner puts the robot in very a difficult situations, and the robot gets stuck easily while there is a clear solution to the movement problem. Instead of following the path, it likes to stick to obstacles, and get itself stuck. When stuck, it keeps oscillating instead of move in reverse and correct its path. I do not penalize negative_x movement because the robot can see its behind pretty good. I read all the documentations for all sections of move_base. I changed a lot of variables to see the effect, but none of them really satisfied me, because the robot gets stuck so easily, and it doesn't get out of it. Below the text are my move_base parameter files (Hopefully it can be also used as a reference for people who are trying to get hydro style move_Base to work with multiple sensors): **The things I have tried:** For DWA robot configuration parameters, I checked the plotted cmd_vel versus the odometry reading. The perfect plot match was with the given parameters below. (I used rqt_plot) - I fiddled around with all the Forward simulation parameters, and the trajectory scoring parameters. - I increased/decreased the inflation radius for both local and global costmap, and increase/decreased their resolution. - I tried changing the reference frame to the centre of the robot instead of centre of rotation. - I fiddled around with controller_frequency and planner and controller patience. - I checked whether the TF matches the robot (with xtions veiwing the robot) - I will prepare a video link and add it here so you can view the problem better. **Things I would like to know** I would greatly appreciate any hint or direction to a better results. Meanwhile I have some more directed questions. 1. How does the dwa_local_planner know about the rotation axis length of the robot? Is it from the footprint? 2. My footprint is a simplified almost rectangular bounding box of the robot. Does it make a difference If I make it precise enough to exactly match the footprint of the robot? 3. Is the Cost value example in the dwa_local_planner webpage that is calculated from the goal_distance , path_distance and occdist values a positive (the high the better path) or negative value? On top of the page it talks about a scoring trajectory but below the example is cost, so I am a bit confused. 4. Do you know why the robot fails while there is a clear solution available? Move_base launch file: Common Costmap params: map_type: costmap footprint: [ [0.1, -0.28], [0.1, 0.28], [-0.58, 0.25], [-0.58, -0.25] ] footprint_padding: 0.0 inflation_radius: 0.2 obstacle_layer_xtions: back_point_cloud_sensor: {clearing: false, data_type: PointCloud2, expected_update_rate: 0.4, obstacle_range: 1.5, raytrace_range: 2, marking: true, min_obstacle_height: 0.1, sensor_frame: back_xtion_link, topic: back_xtion_voxel_grid/output} back_point_cloud_sensor2: {clearing: true, data_type: PointCloud2, expected_update_rate: 0.4, obstacle_range: 1.5, raytrace_range: 2, marking: false, min_obstacle_height: -0.1, sensor_frame: back_xtion_link, topic: back_xtion_voxel_grid/output} front_point_cloud_sensor: {clearing: false, data_type: PointCloud2, expected_update_rate: 0.4, obstacle_range: 2.5, raytrace_range: 3, marking: true, min_obstacle_height: 0.1, sensor_frame: front_xtion_link, topic: front_xtion_voxel_grid/output} front_point_cloud_sensor2: {clearing: true, data_type: PointCloud2, expected_update_rate: 0.4, obstacle_range: 2.5, raytrace_range: 3, marking: false, min_obstacle_height: -0.1, sensor_frame: front_xtion_link, topic: front_xtion_voxel_grid/output} obstacle_layer_laser: laser_scan_sensor: {clearing: true, data_type: LaserScan, expected_update_rate: 0.1, marking: true, obstacle_range: 4, raytrace_range: 6, sensor_frame: laser, topic: scan} Local Costmap: local_costmap: global_frame: map robot_base_frame: base_footprint height: 5 width: 5 origin_x: -4.0 origin_y: -4.0 rolling_window: true publish_frequency: 2.0 resolution: 0.025 static_map: false transform_tolerance: 0.3 update_frequency: 5.0 plugins: - {name: obstacle_layer_xtions, type: 'costmap_2d::ObstacleLayer'} - {name: obstacle_layer_laser, type: 'costmap_2d::ObstacleLayer'} - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} inflation_layer: {cost_scaling_factor: 10.0, enabled: true, inflation_radius: 0.2} obstacle_layer_xtions: observation_sources: front_point_cloud_sensor front_point_cloud_sensor2 back_point_cloud_sensor back_point_cloud_sensor2 enabled: true track_unknown_space: false obstacle_layer_laser: observation_sources: laser_scan_sensor track_unknown_space: true enabled: true obstacle_layer_footprint: {enabled: true} Global Costmap: global_costmap: global_frame: /map robot_base_frame: base_footprint #Height and width of costmap height: 15 width: 15 origin_x: -7 origin_y: -7 publish_frequency: 0.5 resolution: 0.05 transform_tolerance: 0.5 update_frequency: 1.0 plugins: - {name: static_layer, type: 'costmap_2d::StaticLayer'} - {name: obstacle_layer_laser, type: 'costmap_2d::ObstacleLayer'} - {name: obstacle_layer_xtions, type: 'costmap_2d::ObstacleLayer'} - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} static_layer: {enabled: true} static_map: true inflation_layer: {cost_scaling_factor: 10.0, enabled: true, inflation_radius: 0.2} obstacle_layer_xtions: observation_sources: front_point_cloud_sensor front_point_cloud_sensor2 back_point_cloud_sensor back_point_cloud_sensor2 track_unknown_space: true enabled: true obstacle_layer_laser: observation_sources: laser_scan_sensor track_unknown_space: true enabled: true obstacle_layer_footprint: {enabled: true} dwa_local_planner params: DWAPlannerROS: #Robot Config Params acc_lim_theta: 0.35 acc_lim_x: 0.3 acc_lim_y: 0.0 acc_limit_trans: 0.35 max_vel_x: 0.25 min_vel_x: -0.1 max_vel_y: 0.0 min_vel_y: 0.0 max_trans_vel: 0.25 min_trans_vel: 0.01 max_rot_vel: 0.25 min_rot_vel: 0.01 #Forward Simulation Parameters sim_time: 10 sim_granularity: 0.025 vx_samples: 30 vy_samples: 0 vtheta_samples: 40 penalize_negative_x: false #Trajectory Scoring Parameters goal_distance_bias: 32.0 path_distance_bias: 24 occdist_scale: 0.01 stop_time_buffer: 0.2 forward_point_distance: 0.325 scaling_speed: 0.1 max_scaling_factor: 0.2 #Goal Tolerance Parameters xy_goal_tolerance: 0.2 yaw_goal_tolerance: 0.17 latch_xy_goal_tolerance: true rot_stopped_vel: 0.001 trans_stopped_vel: 0.001 #Oscillation Prevention Param oscillation_reset_dist: 0.05 #Global Plan PArameters prune_plan: true

Viewing all articles
Browse latest Browse all 667

Trending Articles



<script src="https://jsc.adskeeper.com/r/s/rssing.com.1596347.js" async> </script>