AMCL and particle filter implementation with IMU and laser range
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
Asked by Astronaut on 2012-11-30 19:57:52 UTC
Comments