Navigation problem

[aviadoz]

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:

costmap_common_params:

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
  }

local_costmap_params:

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"}

base_local_planner_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 (move_base_simple/goal), the move_base 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 diff_robot and I set false in the "holomonic robot" but 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.

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.

Here a  pic from Rviz:
 

I don't know where is the problem. I tried many configurations until now.
I glad if someone could help me pls.


EDIT: I noticed that when I echo to /move_base/TrajectoryPlannerROS/global_plan and to /move_base/TrajectoryPlannerROS/local_plan the fram_id on both is "odom". Maybe it is a problem, I don't know.
tnx

 

[matlabbe]

See https://answers.ros.org/question/310984/navigation-issue/

[aviadoz]

Hi,
Thank you. I saw it.
I think that my problem is with my base_footprint frame.
I will try to solve it and update here if it is works.

[Shalutha Rajapakshe]

I have a d435 camera and using rtabmap with rgbd odometry I was able to make a map of my room.

So after mapping how did you load your saved map  into rviz ? I'm not clear on that point. I already generated a 2d map as well using the pointclouds.

And I believe I can use rgbd odometry in navigation as well? If you know any way to do that please let me know.

Also can you share the launch files you ran in terminals with the order

Thank you