AMCL and Laser_Scan_Matcher [closed]

edit

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 ...