AMCL and Laser_Scan_Matcher [closed]
Hello
I have some doubt regarding using amcl and laser_scan_matcher. As already posted a question here I have some problems wiht my robot motion estimation and the outcoming results of my nodes like estimated angular and linear velocity profiles. My sensor package consist of IMU, Laser range finder and Camera(Kinect). So I do not have any encoders so my odometry is all laser based. For robot localisation Im using AMCL . I have question if Im using it correct since can not identify the role of IMU here. So also implemented the laser scan matcher package as a part of the scan_tools package. So that package should be ok. Just do not see the conection from IMU to AMCL and if Im using it ok. Any help? Here is my amcl node. Also Im posting the node where Im calculating the velocity
AMCL node
class amcl_pose_listener {
public:
ros::NodeHandle n_;
//Message filters for subscribed sensor msgs
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> pose_sub_; //Subscriber to a published pose msg (from a localiser such as AMCL, etc)
ros::Publisher traj_pub_; //Publish an trajectory
//The trajectory
visualization_msgs::Marker trajectory_points_;
amcl_pose_listener(ros::NodeHandle n):
pose_sub_(n_, "amcl_pose", 100) //Subscriber to a pose msg "amcl_pose" (from AMCL)
{
//Set call back function for pose data
pose_sub_.registerCallback(boost::bind(&amcl_pose_listener::poseCallBack, this, _1));
//Publish a trajectory
traj_pub_ = n.advertise<visualization_msgs::Marker>("robot_fixed_pose_trajectory", 100);
}
//Pose data callback
void poseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& data_in)
{
//Init the trajectory history
trajectory_points_.header.frame_id = "map";
trajectory_points_.header.stamp = ros::Time::now();
trajectory_points_.ns = "robot_fixed_pose_trajectory";
trajectory_points_.type = visualization_msgs::Marker::LINE_STRIP;
trajectory_points_.action = visualization_msgs::Marker::ADD;
trajectory_points_.id = 0;
//Green colour
trajectory_points_.color.g = 1.0f;
trajectory_points_.color.a = 1.0;
trajectory_points_.scale.x = 0.1; //Sets the width of the LINE_STRIP
trajectory_points_.scale.y = 0.1; //Ignored if marker type is LINE_STRIP
geometry_msgs::Point p;
p.x = data_in->pose.pose.position.x;
p.y = data_in->pose.pose.position.y;
p.z = data_in->pose.pose.position.z;
//ROS_INFO("POSE RECEIVED\n POSE RECEIVED, seq = %d", trajectory_points_.header.seq);
//ROS_INFO("POSE INIT: %f, %f, %f\n", trajectory_points_.pose.position.x, trajectory_points_.pose.position.y, trajectory_points_.pose.position.z);
trajectory_points_.header.seq++;
trajectory_points_.points.push_back(p); //Add the point
traj_pub_.publish(trajectory_points_); // Publish the trajectory
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "amcl_pose_listener");
ros::NodeHandle n;
amcl_pose_listener lstopc(n);
ros::spin();
return 0;
}
Node for Velocity calculation
v
oid speedCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
btScalar r,p,y;
btQuaternion q(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
btMatrix3x3(q).getEulerYPR(y,p,r);
std_msgs::Float64 yaw;
yaw.data = y;
if (count2 == 0){
time_prev = ros::Time::now().toSec();
count2=1;
}
double time_now = ros::Time::now().toSec();
double curr_theta = yaw.data;
double curr_x = msg->pose.pose.position.x;
double curr_y = msg->pose.pose.position.y;
if (curr_x < 0){
curr_x = curr_x * -1;
}
if (curr_y < 0){
curr_y = curr_y * -1;
}
double time_dif = time_now - time_prev;
if (prev_x != 0 || prev_y != 0){
if (time_dif != 0){
double dist_theta ...