Ask Your Question
0

AMCL: cannot match well, jumps crazily...

asked 2017-03-09 20:28:26 -0500

Bill5785 gravatar image

updated 2017-03-12 22:30:00 -0500

I load the map (built by hector slam before) by map_server and my scan is in rosbag. When I play rosbag. The scan is in "scan" frame while the map is in "map" frame. How to solve the problem?

I know that I also need odometry. But I dont understand how can this get a tf tree? AMCL only provide odom->map. Its not enough to get scan->map.

I want to use AMCL to localize in the given map.

------------------------------------ update ----------------------------------------

Now the tf tree is right. But scan jumps a lot in the map, causing AMCL not able to work. Anyone has idea?

Here is my code sending tf(odom->base_link) based on odometry data.

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>

void Posecallback(const nav_msgs::Odometry::ConstPtr & odom_msg)
{
    static tf::TransformBroadcaster br;
    tf::Transform odom2body;
    tf::Quaternion q(odom_msg->pose.pose.orientation.x,
                     odom_msg->pose.pose.orientation.y,
                     odom_msg->pose.pose.orientation.z,
                     odom_msg->pose.pose.orientation.w);

    odom2body.setOrigin(tf::Vector3(odom_msg->pose.pose.position.x,
                                    odom_msg->pose.pose.position.y,
                                    odom_msg->pose.pose.position.z));
//    q.setRPY(odom_msg->pose.pose.orientation.x,
//             odom_msg->pose.pose.orientation.y,
//             odom_msg->pose.pose.orientation.z,
//             odom_msg->pose.pose.orientation.w);
    odom2body.setRotation(q);
    br.sendTransform(tf::StampedTransform(odom2body,ros::Time::now(),"scanmatch_odom","base_link"));

}



int main(int argc, char ** argv )
{
    ros::init(argc,argv,"odom2base_TF");
    ros::NodeHandle n;
    ros::Subscriber odom_sub=n.subscribe("scanmatch_odom",50,&Posecallback);
    ros::spin();
    return 0;
}

the launch file is like this:

<launch>
<!--include file="/home/wbw/catkin_ws/src/navigation/robot_pose_ekf/launch/test.launch"/-->

<node name="map_server" pkg="map_server" type="map_server" args="/home/wbw/mydata/mymap.yaml"/>

<node pkg="amcl" type="amcl" name="amcl" output="screen">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="diff"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="100"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="2"/>
  <param name="odom_alpha2" value="2"/>
  <param name="odom_alpha3" value="2"/>
  <param name="odom_alpha4" value="2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="resample_interval" value="0.5"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>
    <param name="odom_frame_id" value="scanmatch_odom"/>
    <param name="use_map_topic" value="true"/>
    <param name="initial_cov_xx" value="5.0*5.0"/>
    <param name="initial_cov_yy" value="5.0*5.0"/>
    <param name="initial_pose_x" value="0.0"/>
    <param name="initial_pose_y" value="0.0"/>

</node>

<node pkg ...
(more)
edit retag flag offensive close merge delete

Comments

Jumps are often caused by amcl, maybe run "rosrun tf tf_echo" on both transforms to see which transform is actually jumping.

Humpelstilzchen gravatar image Humpelstilzchen  ( 2017-03-11 14:12:43 -0500 )edit

@Humpelstilzchen I have make sure that its caused by amcl dismatch. I have uploaded a picture in my question. I dont know why amcl cannot match laser

Bill5785 gravatar image Bill5785  ( 2017-03-11 20:37:45 -0500 )edit

Looks like you need some amcl tuning. This is a bit outside my expertise but setting odom_alpha1-4 in amcl is a good start. (Also look at odom_model_type->diff-corrected when setting odom_alpha).

Humpelstilzchen gravatar image Humpelstilzchen  ( 2017-03-12 04:11:47 -0500 )edit
1

Also make sure your odom works!

Humpelstilzchen gravatar image Humpelstilzchen  ( 2017-03-12 04:12:11 -0500 )edit

@Humpelstilzchen Thank you! I tried but nothing changes. Could you please help me check? all my files have been uploaded.

Bill5785 gravatar image Bill5785  ( 2017-03-12 21:55:19 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-03-10 01:35:20 -0500

mgruhler gravatar image

You will always get your sensor information in the sensor frame. Thus, you need to set up your robot correctly for doing localization.

I.e., you need to have a correct tf tree specifiying the robots transformations (where is the sensor w.r.t the base_link) and you need to publish the odometry to tf as well.

There are thorough descriptions in the ROS navigation Tutorials about how to set up your robot correctly

edit flag offensive delete link more

Comments

My problem has been updated. Could you please have a look?

Bill5785 gravatar image Bill5785  ( 2017-03-10 16:26:13 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-03-09 20:28:26 -0500

Seen: 1,808 times

Last updated: Mar 12 '17