Robotics StackExchange | Archived questions

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

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

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