AMCL and particle filter implementation with IMU and laser range [closed]

edit

Hello

I have one question regarding of using the amcl. I have create a node that piblish the pose using the particle filter. But Im not using the IMU datas in the particle filter. I wont the know how to use the IMU and sensor (laser range finder) that to have the best position estimation. My sensor package is only IMU, laser and Camera. So I do not have any encoders( odometry and will not also have it in the future. Any help regarding the existing hardware I have? Im using the IMU only for the ICP initialisation in the laser scan node package.

This is my 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;
}

Any help