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="tf" type="static_transform_publisher" name="base_link_to_laser"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /velodyne 50">
</node>
<!--node pkg="odom2base" type="odom2base_node" name="odom2base" output="screen"/-->
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
<remap from="odom" to="scanmatch_odom"/>
<param name="fixed_frame" value = "scanmatch_odom"/>
<param name="base_frame" value = "base_link"/>
<param name="use_cloud_input" value="false"/>
<param name="publish_tf" value="true"/>
<param name="publish_odom" value="true"/>
<param name="use_odom" value="false"/>
<param name="use_imu" value="false"/>
<param name="use_alpha_beta" value="true"/>
<param name="max_iterations" value="10"/>
</node>
<param name="/use_sim_time" value="true"/>
<node pkg="rosbag" type="play" name="play"
args="/home/wbw/mydata/odomscan.bag --clock">
</node>
</launch>
The dismatch is like this...
the bag file is here (/scan /odom)
my map is here
Originally posted by Bill5785 on ROS Answers with karma: 141 on 2017-03-09
Post score: 0
Original comments
Comment by Humpelstilzchen on 2017-03-11:
Jumps are often caused by amcl, maybe run "rosrun tf tf_echo" on both transforms to see which transform is actually jumping.
Comment by Bill5785 on 2017-03-11:
@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
Comment by Humpelstilzchen on 2017-03-12:
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).
Comment by Humpelstilzchen on 2017-03-12:
Also make sure your odom works!
Comment by Bill5785 on 2017-03-12:
@Humpelstilzchen Thank you! I tried but nothing changes. Could you please help me check? all my files have been uploaded.
