Navigation issue
Hello,
I can't figure out why my real robot does not stop when it arrives close to its goal. that is my configurations:
I have kinetic distro, ubuntu 16.04 real_sense d435 to make Laserscan topic, encoders for the odometry.
main launch file:
<?xml version="1.0"?>
<launch>
<arg name="move_base" default="true" />
<!--load platform 3d model-->
<!--<include file="$(find arl_description)/launch/newurdf.launch" />-->
<!-- bringup realsense camera -->
<group ns="camera">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml"/>
</group>
<!--camera tf-->
<node pkg="tf" type="static_transform_publisher" name="camera_link_arl" args="0.15 0 0.2 0 0 0 base_link camera_link 30" />
<!-- Laser topic -->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
<remap from="image" to="/camera/depth/image_rect_raw"/>
<remap from="camera_info" to="/camera/depth/camera_info"/>
</node>
<!-- encoders odometry -->
<include file="$(find arl_description)/launch/physical_odometry.launch" />
<!--load rtabmap mapping-->
<include file="$(find arl_description)/launch/rtabmap_realsense.launch" >
<arg name="localization" default="true"/>
<arg name="mapping" value="false" />
<arg name="rtabmapviz" default="false" />
<arg name="rviz" default="true" />
<arg name="frame_id" value="base_link" /> <!--kkk-->
<arg name="rgb_topic" default="/camera/color/image_raw" />
<arg name="depth_topic" default="/camera/aligned_depth_to_color/image_raw" /><!--was aligned-->
<arg name="camera_info_topic" default="/camera/color/camera_info" />
</include>
<!-- ROS navigation stack move_base -->
<node if="$(arg move_base)" pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
<rosparam file="$(find arl_description)/launch/nav_data/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find arl_description)/launch/nav_data/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find arl_description)/launch/nav_data/local_costmap_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find arl_description)/launch/nav_data/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find arl_description)/launch/nav_data/base_local_planner_params.yaml" command="load" />
<param name="recovery_behavior_enabled" value="true" />
<remap from="cmd_vel" to="cmd_vel"/>
<remap from="odom" to="odom"/>
<!--<remap from="move_base_simple/goal" to="rtabmap/goal"/>-->
<remap from="scan" to="scan"/>
<remap from="map" to="/grid_map"/>
</node>
<arg name="rviz_cfg" default="$(find arl_description)/launch/realsenseARL.rviz" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>-->
</launch>
my rtabmap launch file(it is the main things):
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg delete_db)">
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<!--<param name="subscribe_scan_cloud" type="bool" value="true"/>-->
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<!-- <param name="publish_tf" type="bool" value="$(arg publish_map_tf)" /> -->
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="odom" to="/odom"/><!-- to get the odometry from the encoders and not from the kinect-->
<remap from="scan" to="/scan"/>
<remap from="scan_map" to="cloud_map"/>
<!--<remap from="grid_map" to="/map"/>-->
<param name="approx_sync" type="bool" value="true"/>
<!-- <param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/> -->
<!-- RTAB-Map's parameters -->
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="RGBD/OptimizeSlam2d" type="string" value="true"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="queue_size" type="int" value="50"/>
<param name="RGBD/PoseScanMatching" type="string" value="true"/>
<param name="RGBD/PlanStuckIterations" type="int" value="10"/>
<param name="RGBD/OptimizeStrategy" type="string" value="1"/> <!-- g2o=1, GTSAM=2 -->
<param name="RGBD/OptimizeRobust" type="string" value="true"/>
<param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!-- should be 0 if RGBD/OptimizeRobust is true -->
<param name="use_action_for_goal" type="bool" value="true"/>
</node>
my move_base yamls:
costmapcommonparams:
footprint: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.02
inflation_layer:
inflation_radius: 0.5
transform_tolerance: 2
obstacle_layer:
obstacle_range: 2.5
raytrace_range: 3
max_obstacle_height: 1
track_unknown_space: true
observation_sources: laser_scan_sensor
laser_scan_sensor: {
data_type: LaserScan,
topic: scan,
expected_update_rate: 0.1,
marking: true,
# min_obstacle_height: 0,
# max_obstacle_height: 2.5,
clearing: true
}
localcostmapparams:
global_frame: odom
robot_base_frame: base_link
update_frequency: 10
publish_frequency: 1
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.025
origin_x: 0
origin_y: 0
static_map: true
plugins:
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
global_costmap_params:
global_frame: map
robot_base_frame: base_link
update_frequency: 10
static_map: true
publish_frequency: 1
width: 3.0
height: 3.0
always_send_full_costmap: false
plugins:
- {name: static_layer, type: "rtabmap_ros::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
baselocalplanner_params:
TrajectoryPlannerROS:
max_vel_x: 0.15
min_vel_x: 0.1
# max_vel_y: 0.0 # diff drive robot
# min_vel_y: 0.0 # diff drive robot
max_vel_theta: 0.15
min_vel_theta: 0.1
min_in_place_vel_theta: 0.1
xy_tolrence_goal: 0.7
yaw_tolerance_goal: 0.3
acc_lim_theta: 0.5
acc_lim_x: 0.3
acc_lim_y: 0.3
holonomic_robot: false
daw: true
meter_scoring: true
sim_time: 1.5
angular_sim_granularity: 0.05
vx_samples: 12
vtheta_samples: 20
pdist_scale: 0.7 # The higher will follow more the global path.
gdist_scale: 0.8
occdist_scale: 0.01
publish_cost_grid_pc: false
NavfnROS:
allow_unknown: true
visualize_potential: false
controller_frequency: 3
I mapped the lab and then I launched all in localization mode. my robot is localized well I think (I can see it when I move in the saved map and It is in the right position and orientation). When I am giving a goal in Rviz (movebasesimple/goal), the movebase compute a path( only global path shown) and the robot is moving. Its seems that it move near the path (I don't know if it really following the path) or it move randomaly. often it rotate in place for recovery. I used a diffrobot and I set false in the "holomonic robot" but I saw that movebase sent cmdvel commands at both linear.x and angular.z at same time. How I can set that it send the commands not in the same time.
I tried to sent a goal by rtabmap/goal and the navigation was the same.
When I am in debugging mode I get these log of rtabmap:
[DEBUG] [1545126342.769918367]: Incoming queue was full for topic "/odom". Discarded oldest message (current queue size [0])
[DEBUG] [1545126342.816679111]: Getting status over the wire.
[ INFO] [1545126342.826370917]: rtabmap (419): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1014s, Maps update=0.0005s pub=0.0004s (local map=58, WM=58)
[DEBUG] [1545126342.954654334]: Incoming queue was full for topic "/scan". Discarded oldest message (current queue size [0])
[DEBUG] [1545126343.017272988]: Getting status over the wire.
[DEBUG] [1545126343.217662035]: Getting status over the wire.
[DEBUG] [1545126343.222661907]: Incoming queue was full for topic "/scan". Discarded oldest message (current queue size [0])
[DEBUG] [1545126343.389663568]: Incoming queue was full for topic "/camera/color/image_raw". Discarded oldest message (current queue size [0])
[DEBUG] [1545126343.389807730]: Incoming queue was full for topic "/camera/color/camera_info". Discarded oldest message (current queue size [0])
[DEBUG] [1545126343.399431744]: Incoming queue was full for topic "/camera/aligned_depth_to_color/image_raw". Discarded oldest message (current queue size [0])
After the robot was localized I cover the camera and I saw that its move almost the same when the camera was not cover. Again,when I gives a different and far goals the robot moves very close cut does't stop, keeps rotate and move a little bit forward and backward.
I don't know what is the problem. I tried many configurations until now. I think maybe the goal that I give move together with the map and because of that the robot arrive near the goal but continue search for the new one.
I glad if someone could help me pls.
EDIT: I noticed that when I echo to /movebase/TrajectoryPlannerROS/globalplan and to /movebase/TrajectoryPlannerROS/localplan the fram_id on both is "odom". Maybe it is a problem, I don't know.
tnx
Update:
yes, you are right but it is not the problem unfortunately (about the spelling). I noticed to another weird issue. I set the minveltheta to 0.1 and values under that were sent. Maybe movebase doesn't takes the values from the yaml although I saw in "rostopic echo /movebase/parameters" that the values are like I set actually.
I think that the map moves when the robot moves and the goal position stay. I am checking it now. It comes very close and when is arrive to its goal it is look like it is moving around the goal. Do you have any suggestion for that?
Asked by Aviad on 2018-12-18 07:56:27 UTC
Answers
You've misspelled "tolerance" in the xy goal tolerance setting so it's not clear what the value for the actual parameter is. I don't know what the default value is. Maybe correcting the spelling is all you need to fix it.
Otherwise you can tune the tolerances of how close you get to the goal position before deciding the goal is reached. It is in these parameters:
xy_tolerance_goal:
yaw_tolerance_goal:
Regarding your comment:
I saw that move_base sent cmd_vel commands at both linear.x and angular.z at same time. How I can set that it send the commands not in the same time.
For differential drive robot it is perfectly acceptable for both X and Z velocities to be issued together. It means the robot is turning as it is driving. That is the way it should be.
Asked by billy on 2018-12-18 18:39:09 UTC
Comments
Use rqt_reconfigure to see parameters actually used by move_base. You can also change them live to debug. I think
min_vel_theta
should be negative (turning left). See also http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide.Asked by matlabbe on 2018-12-21 15:24:22 UTC