i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated azimut3 nav parameters, fixed last goal not sent when distance between last goal and transform to goal is < goalReachedRadius

Browse Source
This commit is contained in:
matlabbe
2015-03-18 18:00:30 -04:00
parent a0ae280b74
commit c58a11da0b
3 changed files with 29 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/az3_mapping_robot_nav.launch
+18 -25
View File
@@ -14,9 +14,9 @@
<group ns="rtabmap"> <group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)"> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/> <param name="subscribe_laserScan" type="bool" value="true"/>
<param name="use_action_for_goal" type="bool" value="true"/>
<param name="cloud_decimation" type="int" value="1"/> <!-- we already decimate in memory below -->
<remap from="odom" to="/base_controller/odom"/> <remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/> <remap from="scan" to="/base_scan"/>
@@ -30,44 +30,37 @@
<remap from="move_base" to="/planner/move_base"/> <remap from="move_base" to="/planner/move_base"/>
<remap from="proj_map" to="/map"/> <remap from="proj_map" to="/map"/>
<param name="use_action_for_goal" type="bool" value="true"/> <!-- RTAB-Map's parameters -->
<param name="queue_size" type="int" value="10"/>
<param name="cloud_decimation" type="int" value="1"/>
<param name="RGBD/PoseScanMatching" type="string" value="true"/> <param name="RGBD/PoseScanMatching" type="string" value="true"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
<param name="LccIcp/Type" type="string" value="2"/> <param name="LccIcp/Type" type="string" value="2"/>
<param name="LccIcp2/Iterations" type="string" value="100"/>
<param name="LccIcp2/VoxelSize" type="string" value="0"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving --> <param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving --> <param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LocalRadius" type="string" value="5"/> <param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/> <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/> <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="Mem/ImageDecimation" type="string" value="4"/> <param name="Mem/ImageDecimation" type="string" value="4"/>
<param name="Mem/LocalSpaceLinksKeptInWM" type="string" value="true"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Rtabmap/TimeThr" type="string" value="600"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/ToroIterations" type="string" value="100"/> <param name="Bayes/PredictionLC" type="string" value="0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.00035"/>
<param name="RGBD/OptimizeSlam2d" type="string" value="true"/>
<param name="RGBD/OptimizeIterations" type="string" value="100"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Kp/TfIdfLikelihoodUsed" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="Kp/WordsPerImage" type="string" value="200"/> <param name="Kp/WordsPerImage" type="string" value="200"/>
<param name="Kp/NNStrategy" type="string" value="1"/> <param name="Kp/NNStrategy" type="string" value="1"/>
<param name="Bayes/FullPredictionUpdate" type="string" value="false"/> <param name="SURF/HessianThreshold" type="string" value="500"/>
<param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="LccBow/Force2D" type="string" value="true"/> <param name="LccBow/Force2D" type="string" value="true"/>
<param name="LccBow/MaxDepth" type="string" value="4"/> <param name="LccBow/MaxDepth" type="string" value="5"/>
<param name="LccBow/MinInliers" type="string" value="5"/> <param name="LccBow/MinInliers" type="string" value="5"/>
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <param name="LccBow/InlierDistance" type="string" value="0.1"/>
</node> </node>
@@ -88,7 +81,7 @@
<remap from="map" to="/map"/> <remap from="map" to="/map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/> <remap from="move_base_simple/goal" to="/planner_goal"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_laser.yaml" command="load" /> <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_laser.yaml" command="load" />
@@ -126,10 +119,10 @@
<!-- for the planner --> <!-- for the planner -->
<node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager"> <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
<remap from="depth/image" to="data_resized_image_depth"/> <remap from="depth/image" to="data_resized_image_depth"/>
<remap from="depth/camera_info" to="data_resized_camera_info"/> <remap from="depth/camera_info" to="data_resized_camera_info"/>
<remap from="cloud" to="/local_costmap_cloud" /> <remap from="cloud" to="/local_costmap_cloud" />
<param name="decimation" type="int" value="1"/> <param name="decimation" type="int" value="1"/> <!-- already decimated above -->
<param name="max_depth" type="double" value="6.0"/> <param name="max_depth" type="double" value="6.0"/>
<param name="noise_filter_radius" type="double" value="0.05"/> <param name="noise_filter_radius" type="double" value="0.05"/>
<param name="noise_filter_min_neighbors" type="int" value="10"/> <param name="noise_filter_min_neighbors" type="int" value="10"/>
launch/azimut3/config/base_local_planner_params.yaml
+7 -7
View File
@@ -8,23 +8,23 @@ TrajectoryPlannerROS:
# minimal distance of 0.48 m. # minimal distance of 0.48 m.
# Basically, max_rotational_vel * rho_min <= min_vel_x # Basically, max_rotational_vel * rho_min <= min_vel_x
max_vel_x: 0.700 max_vel_x: 0.700
min_vel_x: 0.212 min_vel_x: 0.24
max_rotational_vel: 0.550 max_rotational_vel: 0.5
min_in_place_rotational_vel: 0.15 min_in_place_rotational_vel: 0.15
escape_vel: -0.10 escape_vel: -0.10
holonomic_robot: false holonomic_robot: true
xy_goal_tolerance: 0.20 xy_goal_tolerance: 0.20
yaw_goal_tolerance: 0.20 yaw_goal_tolerance: 0.20
sim_time: 1.7 sim_time: 1
sim_granularity: 0.025 sim_granularity: 0.025
vx_samples: 3 vx_samples: 3
vtheta_samples: 3
vtheta_samples: 20 vtheta_samples: 20
controller_frequency: 10
goal_distance_bias: 0.8 pdist_scale: 0.6
path_distance_bias: 0.6 gdist_scale: 0.8
occdist_scale: 0.01 occdist_scale: 0.01
heading_lookahead: 0.325 heading_lookahead: 0.325
dwa: true dwa: true
src/CoreWrapper.cpp
+4 -2
View File
@@ -1070,7 +1070,9 @@ void CoreWrapper::process(
bool lastPoseModified = false; bool lastPoseModified = false;
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size()) if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
{ {
if(latestNodeWasReached_ || rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius()) if(latestNodeWasReached_ ||
rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() ||
rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius())
{ {
if(!latestNodeWasReached_) if(!latestNodeWasReached_)
{ {
@@ -2372,7 +2374,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
{ {
if(rtabmap_.getPath().size() && if(rtabmap_.getPath().size() &&
rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first && rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first)) (!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_))
{ {
ROS_WARN("Planning: move_base reached current goal but it is not " ROS_WARN("Planning: move_base reached current goal but it is not "
"the last one planned by rtabmap. A new goal should be sent when " "the last one planned by rtabmap. A new goal should be sent when "