Robot stuck in it own footprint

[thanhnguyen]

Hi Mathieu,

Because the old one is too many questions so I change to this post to continue as question, sorry:))


1/


In this picture, my robot think it footprint is an obstacle, becasue the sick laser-scanner mount very low. Therefore, when I just create a map, my room is full obstacle and all of them is my robot footprint. I try do set a min_obstacle_height in costmap_commond parameter but nothing change.

2/ related to (1). Because of this problem, I decide not use move_base_simple but change to rtabmap/goal. But went I change to rtabmap/goal, the robot not move, just simple not move. although I already set "RGBD/OptimizeFromGraphEnd"=true. Here is the picture:



and when I go to rqt_graph, I see the rtabmap/goal doesn't link to move_base topic, that why the robot can not move:



3/ Finally, I try to modify files az3_nav to use in my robot volksbot, I change az3_bringup to volksbot_bringup, you can see in the file below:

az3_nav.launch

I got result like this, the robot can display the map but it dont know it odometry in the map, here is the result



And when I sen 2D navigation, robot not move because it dont know the odometry

[matlabbe]

Hi,

If your robot is already on an obstacle, move_base won't let you plan a new trajectory. The obstacle on the robot are maybe the laser hitting the robot parts. You can filter the laser scans with laser_filters to remove valus below a fixed range (use LaserScanRangeFilter), removing obstacles seen on the robot.

You can also enable footprint clearing in the costmap parameters. Don't remember exactly the name of this parameter, but you can see it when launching rqt_reconfigure. However, if the robot really steps on an obstacle, it could continue to send commands toward it, which can be bad.

The question is "is rtabmap/goal_out" published? If not, look at the terminal for warnings/errors when you send a goal on /rtabmap/goal.

cheers

[thanhnguyen]

Dear Mathieu,                                                                       Thank you very much! I change min range of the laser scaner in files lms1xx.cpp higher so the robot not detect itself any more. Thanks for your advise!!                              I a question about the odometry. I mounted the kinect camera on my robot, so I have two odometries, one from my wheel encoder and one from the kinect camera. But when the robot turn right or left too fast, the kinect lost odometry and the program is break. I would like to ask how the kinect can define the odometry? Because follow what I know the wheels encoder define odometry base on Motor ticks, so how kinect can define odometry.                            2. One more question, are there any way to automatically reset odometry? Because when I make the robot move with move-base with navigation stack, if the robot move so fast or near the obstacles, the camera loose odometry and the program breaks so the robot just turn around.                    Thanks you in advance

[matlabbe]

Hi,

1. Visual odometry of rtabmap is done by extracting 2d local features from the rgb images, then use depth image to project them in 3D. With 3D->3D or 2D->3D correspondences between frames, we can estimate the motion.

2. Set parameter "Odom/ResetCountdown" to 1 for rgbd_odometry node:

rtabmap --params | grep Reset
Param: Odom/ResetCountdown = "0"    [Automatically reset odometry after X consecutive images 
                                     on which odometry cannot be computed (value=0 disables 
                                     auto-reset).]

Note that if wheel odometry is relatively accurate, you may not use visual odometry to avoid being lost. See "Kinect + Odometry + 2D laser" setup for example using also a lidar.

cheers,
Mathieu

[thanhnguyen]

Hi,

I already add lis line in rtabmap.launch and save this file

      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)"
         args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
       
        ""param name="Odom/ResetCountdown"  type="string" value="1"/>""

But when I type rtabmap --params | grep Reset in terminal, the value still 0, why??



Best Regard.

[matlabbe]

Hi,

using --params on rtabmap shows only default parameters. To see actual value set, you may use "rosparam list" and "rosparam get".

 <param name="Odom/ResetCountdown"  type="string" value="1"/>

[thanhnguyen]

Hi Mathieu


When I run the rgbd_mapping.launch it got this warning, rejected loop closeure 24-115


And when i run move base on file navigation.launch, I saw this warning

control loop missed its desired rate of 15.0000Hz... the loop actually took 0.0826 second



Then robot draw absolutely wrong map. Could you help me?

[matlabbe]

Hi,

The warning "control loop missed its desired rate of" is not coming from rtabmap. It may be the control loop of move_base, which is not "loop closure", but used to issue velocity commands at 15 Hz. Look at the CPU usage, if you have 100%, this may be the problem.

The rejected loop closure warning is just telling that it found a match, but could not compute the transformation because there are not enough visual inliers. This can be ignored most of the time, unless there is an obvious loop closure.

"The robot draw absolutely wrong map", do you have a screen shot?

cheers

[thanhnguyen]

Hi Mathieu,

I fixed that problem,

I want to ask you about the odometry, what happen if I dont want to use rtabmap/rgbd_odometry anymore but I want to use odometry directly from my robot, where should I change in the launch files. My robot is volksbot.

Thanks.

[thanhnguyen]

Hi Mathieu,

In my voksbot_driver.cpp,  I see odom and odom_combined together



In my files, i just remap form "odom" to "odom" , like this


In rqt grahp, I have two odom, 1 from volksbot and 1 from rtabmap/odom. I don't understand why,



And finally when i run my volskbot_navlaunch, they are seperate tree like this.


The map was create, the robot exit in the map, but it dont know it positon,

Thanks

[matlabbe]

Hi,

please copy paste code between "raw" html tags (see More->Raw text when editing a post in the forum). It is hard to refer some parts when it is an image.

Remove rgbd_odometry node from your launch file first, this will cleanup some TF and topics. Then normally with the odom remap it should be okay as you did, though maybe there are multiple rtabmap started at the same time (as rgbd_odometry is started)?

[thanhnguyen]

Hi Mathieu

This is my launch file of volksbot_nav.launch.

<launch>

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>

  <!-- Volskbot 3 bringup: launch motors/odometry, laser scan and openni -->

  <include file="$(find volksbot_driver)launch/volksbot.launch"/>
  <include file="$(find lms1xx)launch/LMS1xx.launch"/>
  <include file="$(find freenect_launch)/launch/freenect.launch">
     <arg name="depth_registration" value="True" />
  </include>

  <node pkg="tf" type="static_transform_publisher" name="camera" 
   args="0 0 0 0 0 0 base_link camera_link 100" /> 
  <node pkg="tf" type="static_transform_publisher" name="laser" 
   args="0 0 0 0 0 0 base_link laser 100" />
  <node pkg="tf" type="static_transform_publisher" name="odom" 
   args="0 0 0 0 0 0 odom_combined base_footprint 100" /> 
  


  <!-- SLAM (robot side) -->
  <group ns="rtabmap">
    <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="subscribe_scan"      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 -->
      <param name="grid_eroded"         type="bool" value="true"/>
      <param name="grid_cell_size"      type="double" value="0.05"/>

      <remap from="odom"    to="/odom"/>
      <remap from="scan"    to="/scan"/>
      <remap from="mapData" to="mapData"/>

      <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>

      <remap from="goal_out"  to="current_goal"/>	
      <remap from="move_base" to="/planner/move_base"/>
      <remap from="grid_map"  to="/map"/>

      <!-- RTAB-Map's parameters -->
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>           
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
  
      <param name="Reg/Strategy" type="string" value="1"/>                    
	 
      <param name="RGBD/AngularUpdate" type="string" value="0.1"/> 
      <param name="RGBD/LinearUpdate"  type="string" value="0.1"/> 
      <param name="RGBD/LocalRadius"   type="string" value="5"/>
	  
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
      <param name="Mem/NotLinkedNodesKept"  type="string" value="false"/>
      <param name="Mem/ImagePostDecimation"     type="string" value="4"/>
      
      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
      <param name="Rtabmap/TimeThr"                  type="string" value="600"/>
      <param name="Rtabmap/DetectionRate"            type="string" value="1"/>

      <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="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
      <param name="Optimizer/Strategy"        type="string" value="0"/>      
 
      <param name="Kp/DetectorStrategy"   type="string" value="0"/>
      <param name="Kp/MaxFeatures"        type="string" value="200"/>
      <param name="SURF/HessianThreshold" type="string" value="500"/>
	  
      <param name="Reg/Force3DoF"  type="string" value="true"/>
      <param name="Vis/MaxDepth"   type="string" value="5"/>
      <param name="Vis/MinInliers" type="string" value="5"/>
      <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>      

      <!-- 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)"/>
    </node>
  </group>
  
   <!-- teleop -->
  <node pkg="turtlebot_teleop" type="turtlebot_teleop_key" name="turtlebot_teleop_keyboard" output="screen">
  <param name="scale_linear" value="0.5" type="double"/><param name="scale_angular" value="1.5" type="double"/>
  <remap from="turtlebot_teleop_keyboard/cmd_vel" to="cmd_vel"/>
  </node>

  <!-- ROS navigation stack move_base -->
  <group ns="planner">
     <remap from="odom" to="/odom"/>
     <remap from="scan" to="/scan"/>
     <remap from="obstacles_cloud" to="/obstacles_cloud"/>
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="rtabmap/grid_map"/>
     <remap from="move_base_simple/goal" to="/planner_goal"/>
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
     </node>
   		
  </group>
  
  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
      <param name="rate" type="double" value="5"/>
      <param name="decimation" type="int" value="2"/>
   
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
    
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
    </node>

    <!-- for the planner -->
    <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/camera_info"      to="data_resized_camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="1"/>                     <!-- already decimated above -->
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
  
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_footprint"/>		
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="Grid/MinClusterSize"     type="int" value="20"/>
      <param name="Grid/MaxObstacleHeight" type="double" value="0.4"/>
    </node>  
  </group>
</launch>

2/But when I run this launch files", the result will be like this, the laser scan and the kinect_camera create two seperate maps:



3/ And robot do not know it position, when I teleoperate the robot in real enviroment, the robot in rviz not move. And of course the navigation_stack, move_base not work, too.
4/ This is the rqt graph of this launch files, please help me.


Best Regards

[matlabbe]

Hi,

The static transforms look not good:

  <node pkg="tf" type="static_transform_publisher" name="camera" 
   args="0 0 0 0 0 0 base_link camera_link 100" /> 
  <node pkg="tf" type="static_transform_publisher" name="laser" 
   args="0 0 0 0 0 0 base_link laser 100" />
  <node pkg="tf" type="static_transform_publisher" name="odom" 
   args="0 0 0 0 0 0 odom_combined base_footprint 100" /> 

Remove "odom" publisher, this transform should be published in your odometry node (the one publishing odmetry message). See http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

The "camera" and "laser" transform may need to be updated. I don't think the laser is directly on the shaft of the robot (the base frame referring to odometry). The camera is certainly not exactly on the same position than the laser. The rotation between the camera and the lidar map is probably this wrong TF.

cheers

[thanhnguyen]

Hi,

Here is my odometry node, this from the volksbot_odometry. They use "odom_combined" as the parent coordinate frame and "base_footprint" as the child coordinate frame.

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/JointState.h>
#include <ros/ros.h>
#include <ros/console.h>

#include <string>

#include "volksbot.h"

class ROSComm : public Comm
{
  public:
    ROSComm(
        const ros::NodeHandle &n,
        double sigma_x,
        double sigma_theta,
        double cov_x_y,
        double cov_x_theta,
        double cov_y_theta) :
      n_(n),
      sigma_x_(sigma_x),
      sigma_theta_(sigma_theta),
      cov_x_y_(cov_x_y),
      cov_x_theta_(cov_x_theta),
      cov_y_theta_(cov_y_theta),
      publish_tf_(false),
      odom_pub_(n_.advertise<nav_msgs::Odometry> ("odom", 10)),
      joint_pub_(n_.advertise<sensor_msgs::JointState> ("joint_states", 1)) { }
    virtual void send_odometry(double x, double y, double theta, double v_x,
        double v_theta, double wheelpos_l, double wheelpos_r);

    void setTFPrefix(const std::string &tf_prefix);

  private:
    void populateCovariance(nav_msgs::Odometry &msg, double v_x, double
        v_theta);

    ros::NodeHandle n_;
    double sigma_x_, sigma_theta_, cov_x_y_, cov_x_theta_, cov_y_theta_;
    bool publish_tf_;
    std::string tf_prefix_;

    tf::TransformBroadcaster odom_broadcaster_;
    ros::Publisher odom_pub_;
    ros::Publisher joint_pub_;
};

void ROSComm::setTFPrefix(const std::string &tf_prefix)
{
  tf_prefix_ = tf_prefix;
}

void ROSComm::populateCovariance(nav_msgs::Odometry &msg, double v_x, double v_theta)
{
  double odom_multiplier = 1.0;

  if (fabs(v_x) <= 1e-8 && fabs(v_theta) <= 1e-8)
  {
    //nav_msgs::Odometry has a 6x6 covariance matrix
    msg.twist.covariance[0] = 1e-12;
    msg.twist.covariance[35] = 1e-12;

    msg.twist.covariance[30] = 1e-12;
    msg.twist.covariance[5] = 1e-12;
  }
  else
  {
    //nav_msgs::Odometry has a 6x6 covariance matrix
    msg.twist.covariance[0] = odom_multiplier * pow(sigma_x_, 2);
    msg.twist.covariance[35] = odom_multiplier * pow(sigma_theta_, 2);

    msg.twist.covariance[30] = odom_multiplier * cov_x_theta_;
    msg.twist.covariance[5] = odom_multiplier * cov_x_theta_;
  }

  msg.twist.covariance[7] = DBL_MAX;
  msg.twist.covariance[14] = DBL_MAX;
  msg.twist.covariance[21] = DBL_MAX;
  msg.twist.covariance[28] = DBL_MAX;

  msg.pose.covariance = msg.twist.covariance;

  if (fabs(v_x) <= 1e-8 && fabs(v_theta) <= 1e-8)
  {
    msg.pose.covariance[7] = 1e-12;

    msg.pose.covariance[1] = 1e-12;
    msg.pose.covariance[6] = 1e-12;

    msg.pose.covariance[31] = 1e-12;
    msg.pose.covariance[11] = 1e-12;
  }
  else
  {
    msg.pose.covariance[7] = odom_multiplier * pow(sigma_x_, 2) * pow(sigma_theta_, 2);

    msg.pose.covariance[1] = odom_multiplier * cov_x_y_;
    msg.pose.covariance[6] = odom_multiplier * cov_x_y_;

    msg.pose.covariance[31] = odom_multiplier * cov_y_theta_;
    msg.pose.covariance[11] = odom_multiplier * cov_y_theta_;
  }
}

void ROSComm::send_odometry(double x, double y, double theta, double v_x, double v_theta, double wheelpos_l, double wheelpos_r)
{
  nav_msgs::Odometry odom;
  odom.header.frame_id = tf::resolve(tf_prefix_, "odom_combined");
  odom.child_frame_id = tf::resolve(tf_prefix_, "base_footprint");

  odom.header.stamp = ros::Time::now();
  odom.pose.pose.position.x = x;
  odom.pose.pose.position.y = y;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);

  odom.twist.twist.linear.x = v_x;
  odom.twist.twist.linear.y = 0.0;
  odom.twist.twist.angular.z = v_theta;
  populateCovariance(odom, v_x, v_theta);

  odom_pub_.publish(odom);

  if (publish_tf_)
  {
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.frame_id = tf::resolve(tf_prefix_, "odom_combined");
    odom_trans.child_frame_id = tf::resolve(tf_prefix_, "base_footprint");

    odom_trans.header.stamp = ros::Time::now();
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(theta);

    odom_broadcaster_.sendTransform(odom_trans);
  }

  sensor_msgs::JointState joint_state;
  joint_state.header.stamp = ros::Time::now();
  joint_state.name.resize(6);
  joint_state.position.resize(6);
  joint_state.name[0] = "left_front_wheel_joint";
  joint_state.name[1] = "left_middle_wheel_joint";
  joint_state.name[2] = "left_rear_wheel_joint";
  joint_state.name[3] = "right_front_wheel_joint";
  joint_state.name[4] = "right_middle_wheel_joint";
  joint_state.name[5] = "right_rear_wheel_joint";

  joint_state.position[0] = joint_state.position[1] = joint_state.position[2] = wheelpos_l;
  joint_state.position[3] = joint_state.position[4] = joint_state.position[5] = wheelpos_r;

  joint_pub_.publish(joint_state);
}

class ROSCall
{
  public:
    ROSCall(Volksbot &volksbot, double axis_length) :
      volksbot_(volksbot),
      axis_length_(axis_length),
      last_cmd_vel_time_(0.0) { }
    void velCallback(const geometry_msgs::Twist::ConstPtr& msg);
    void cmd_velWatchdog(const ros::TimerEvent& event);

  private:
    Volksbot &volksbot_;
    double axis_length_;
    ros::Time last_cmd_vel_time_;
};

void ROSCall::velCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
  last_cmd_vel_time_ = ros::Time::now();
  double max_vel = volksbot_.get_max_vel();
  double velocity = msg->linear.x;

  velocity = std::min(max_vel, msg->linear.x);
  velocity = std::max(-max_vel, msg->linear.x);
  volksbot_.set_wheel_speed(velocity - axis_length_ * msg->angular.z * 0.5, velocity + axis_length_ * msg->angular.z * 0.5);
}

void ROSCall::cmd_velWatchdog(const ros::TimerEvent& event)
{
  if (ros::Time::now() - last_cmd_vel_time_ > ros::Duration(0.6))
    volksbot_.set_wheel_speed(0.0, 0.0);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "volksbot");
  ros::NodeHandle n;
  ros::NodeHandle nh_ns("~");

  /* This is the wheel Radius for the odometry, accounting for some slip in the movement.
   * therefor it's not the same as the one in the volksbot.urdf.xacro */
  double wheel_radius;
  nh_ns.param("wheel_radius", wheel_radius, 0.0985);
  double axis_length;
  nh_ns.param("axis_length", axis_length, 0.41);
  int gear_ratio;
  nh_ns.param("gear_ratio", gear_ratio, 74);
  int max_vel_l;
  nh_ns.param("max_vel_l", max_vel_l, 8250);
  int max_vel_r;
  nh_ns.param("max_vel_r", max_vel_r, 8400);
  int max_acc_l;
  nh_ns.param("max_acc_l", max_acc_l, 10000);
  int max_acc_r;
  nh_ns.param("max_acc_r", max_acc_r, 10000);
  bool drive_backwards;
  nh_ns.param("drive_backwards", drive_backwards, false);

  double turning_adaptation;
  nh_ns.param("turning_adaptation", turning_adaptation, 0.95);

  double sigma_x, sigma_theta, cov_x_y, cov_x_theta, cov_y_theta;
  nh_ns.param("x_stddev", sigma_x, 0.002);
  nh_ns.param("rotation_stddev", sigma_theta, 0.017);
  nh_ns.param("cov_xy", cov_x_y, 0.0);
  nh_ns.param("cov_xrotation", cov_x_theta, 0.0);
  nh_ns.param("cov_yrotation", cov_y_theta, 0.0);

  ROSComm roscomm(n, sigma_x, sigma_theta, cov_x_y, cov_x_theta, cov_y_theta);

  Volksbot volksbot(roscomm, wheel_radius, axis_length, turning_adaptation, gear_ratio, max_vel_l, max_vel_r, max_acc_l, max_acc_r, drive_backwards);

  bool publish_tf;
  nh_ns.param("publish_tf", publish_tf, false);
  std::string tf_prefix;
  tf_prefix = tf::getPrefixParam(nh_ns);
  roscomm.setTFPrefix(tf_prefix);

  ROSCall roscall(volksbot, axis_length);

  ros::Timer timer = n.createTimer(ros::Duration(0.1), &ROSCall::cmd_velWatchdog, &roscall);
  ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 10, &ROSCall::velCallback, &roscall);

  while (ros::ok())
  {
    volksbot.odometry();
    ros::spinOnce();
  }

  return 0;
}

I can not find anything wrong here? But in the tf tree if I Remove "odom" publisher.  The result would be like that. They are not in the same tree.



2/ Yes you are right, I mount the kinnect camera to volksbot, the laser alredy there before. But I choose the direction same with  odometry of the robot, you can see.

[matlabbe]

Set "publish_tf_" to true in your volksbot_odometry node so you will have the missing TF odom_combined -> base_footprint.

[thanhnguyen]

Thank you, it work!!!
Now the robot can localization in the map use it odometry, not odometry from kinect any_more.

I have two question.

1/Question 1: when I sent the goal robot not move, topic of 2D Nav Goal is :/rtabmap/goal. And I check the rqt_graph, here the planner/move_base not have any connect with /cmd_vel, which that should be have if I use rgbd_odometry from kinect.



This is my launch files.

<launch>

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>

  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->

  <include file="$(find volksbot_driver)launch/volksbot.launch"/>
  <include file="$(find lms1xx)launch/LMS1xx.launch"/>
  <include file="$(find freenect_launch)/launch/freenect.launch">
     <arg name="depth_registration" value="True" />
  </include>

  <node pkg="tf" type="static_transform_publisher" name="camera" 
   args="0 0 0 0 0 0 base_footprint camera_link 100" /> 
  <node pkg="tf" type="static_transform_publisher" name="laser" 
   args="0 0 0 0 0 0 base_footprint laser 100" />


  <!-- SLAM (robot side) -->
  <group ns="rtabmap">
    <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="subscribe_scan"      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 -->
      <param name="grid_eroded"         type="bool" value="true"/>
      <param name="grid_cell_size"      type="double" value="0.05"/>

      <remap from="odom"    to="/odom"/>
      <remap from="scan"    to="/scan"/>
      <remap from="mapData" to="mapData"/>

      <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>

      <remap from="goal_out"  to="current_goal"/>	
      <remap from="move_base" to="/planner/move_base"/>
      <remap from="grid_map"  to="/rtabmap/grid_map"/>

      <!-- RTAB-Map's parameters -->
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>           
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
  
      <param name="Reg/Strategy" type="string" value="1"/>                    
	 
      <param name="RGBD/AngularUpdate" type="string" value="0.1"/> 
      <param name="RGBD/LinearUpdate"  type="string" value="0.1"/> 
      <param name="RGBD/LocalRadius"   type="string" value="5"/>
	  
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
      <param name="Mem/NotLinkedNodesKept"  type="string" value="false"/>
      <param name="Mem/ImagePostDecimation"     type="string" value="4"/>
      
      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
      <param name="Rtabmap/TimeThr"                  type="string" value="600"/>
      <param name="Rtabmap/DetectionRate"            type="string" value="1"/>

      <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="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
      <param name="Optimizer/Strategy"        type="string" value="0"/>      
 
      <param name="Kp/DetectorStrategy"   type="string" value="0"/>
      <param name="Kp/MaxFeatures"        type="string" value="200"/>
      <param name="SURF/HessianThreshold" type="string" value="500"/>
	  
      <param name="Reg/Force3DoF"  type="string" value="true"/>
      <param name="Vis/MaxDepth"   type="string" value="5"/>
      <param name="Vis/MinInliers" type="string" value="5"/>
      <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>      

      <!-- 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)"/>
    </node>
  </group>
  
   <!-- teleop -->
  <node pkg="turtlebot_teleop" type="turtlebot_teleop_key" name="turtlebot_teleop_keyboard" output="screen">
  <param name="scale_linear" value="0.5" type="double"/><param name="scale_angular" value="1.5" type="double"/>
  <remap from="turtlebot_teleop_keyboard/cmd_vel" to="cmd_vel"/>
  </node>

  <!-- ROS navigation stack move_base -->
  <group ns="planner">
     <remap from="odom" to="/odom"/>
     <remap from="scan" to="/scan"/>
     <remap from="obstacles_cloud" to="/obstacles_cloud"/>
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="/rtabmap/grid_map"/>
     <remap from="move_base_simple/goal" to="/planner_goal"/>
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
     </node>
   		
  </group>
  
  <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
      <param name="rate" type="double" value="5"/>
      <param name="decimation" type="int" value="2"/>
   
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
    
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
    </node>

    <!-- for the planner -->
    <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/camera_info"      to="data_resized_camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="1"/>                     <!-- already decimated above -->
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
  
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_footprint"/>		
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="Grid/MinClusterSize"     type="int" value="20"/>
      <param name="Grid/MaxObstacleHeight" type="double" value="0.4"/>
    </node>  
  </group>
</launch>

2/Question2: And the the "Map Cloud" which take info from /rtabmap/mapData and the /scan topic from laser still create separate two map. I think the different here is the Laser scanner and the kinect is not a part of the robot, I mount them on the robot and connect them directly to my Linux_Thinkpad.

Are you do the same for your azimut3 or your robot have the camera and laser scanner itself? And in my situation, how to fix this?

[matlabbe]

Hi,

1) Maybe in this code there is some topic remapping required as move_base is in planner namespace, like "/planner/cmd_vel" to "/cmd_vel"

<!-- ROS navigation stack move_base -->
  <group ns="planner">
     <remap from="odom" to="/odom"/>
     <remap from="scan" to="/scan"/>
     <remap from="obstacles_cloud" to="/obstacles_cloud"/>
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="/rtabmap/grid_map"/>
     <remap from="move_base_simple/goal" to="/planner_goal"/>
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
     </node>
   		
  </group>

2) It is not required that the lidar and camera is on the robot, as long as TF between base_link and the lidar/camera frames are accurate. Azimut has lidar and camera carefully aligned together (TF or urdf), along with base_link.

cheers,
Mathieu

[thanhnguyen]

Hi Mathieu my Tutor,

Thanks you very much, it work. I  remapping  "/planner/cmd_vel" to "/cmd_vel" and the robot start moving, I also change the angle of laserscaner to make it aligned with base_link. And here is the result.



Cheers!!!

[matlabbe]

Great! looking good!

[thanhnguyen]

Hi Mathieu,

I still have some question about rtabmap:

1/ Kinnect-camera use for create 3D point cloud. And laser scanner use to create the map, so which algorithm and filter you used to map create?

2/ Can I input the goal position(x,y,z) form keyboard (not use 2D Navigation in Rviz)?

3/ Navigation_Stack node recieve the map form rtabmap/grid_map. Just the map or also position of the robot? I mean rtabmap use SLAM and navigation stack only use this result?

4/Can I export the position of robot and laser scan to a Tex file or an excel file?

Thanks,