void odomCallback(const nav_msgs::Odometry& msg)
{
geometry_msgs::Pose2D old_pose = odo_robot_pose;
// Store updated pose values
odo_robot_pose.x = msg.pose.pose.position.x;
odo_robot_pose.y = msg.pose.pose.position.y;
odo_robot_pose.theta = tf::getYaw(msg.pose.pose.orientation);
// Store velocity computed from odometry
odo_lin_vel = msg.twist.twist.linear.x;
odo_ang_vel = msg.twist.twist.angular.z;
// Store timestamp and update flag
odom_updated = true;
// Only perform the particle filter update step if we have alread received an
// odometry message in the past.
if( !odom_first_update )
{
/// Perform Step 1 of the particle filter
/// --> Update particles with robot movement:
// We need to obtain the robot movement in the robot coordinates
geometry_msgs::Pose2D local_pose;
world2Local(old_pose, odo_robot_pose, &local_pose);
ParticleFilterStep1(local_pose.x, local_pose.theta);
}
// First update is done
odom_first_update = false;
last_step1_time = msg.header.stamp.toSec();
// Show updated particles
showDebugInformation();
}
void TP8::odomCallback(const nav_msgs::Odometry& msg)
{
geometry_msgs::Pose2D old_pose = _odo_robot_pose;
// Store updated pose values
_odo_robot_pose.x = msg.pose.pose.position.x;
_odo_robot_pose.y = msg.pose.pose.position.y;
_odo_robot_pose.theta = tf::getYaw(msg.pose.pose.orientation);
// Store velocity computed from odometry
_odo_lin_vel = msg.twist.twist.linear.x;
_odo_ang_vel = msg.twist.twist.angular.z;
// Store timestamp and update flag
_odom_updated = true;
// Only perform the prediction update step if we have alread received an
// odometry message in the past.
if( !_odom_first_update )
{
/// Perform Step 1 of the particle filter
/// --> Update particles with robot movement:
// We need to obtain the robot movement in the robot coordinates
geometry_msgs::Pose2D local_pose;
world2Local(old_pose, _odo_robot_pose, &local_pose);
_ekf.predictStep(local_pose.x, local_pose.y, local_pose.theta);
} else
{
// Set initial state value to the odometry information
_ekf._state(0,0) = _odo_robot_pose.x;
_ekf._state(1,0) = _odo_robot_pose.y;
_ekf._state(2,0) = _odo_robot_pose.theta;
/// @TODO: use ROS given covariance to initialize P
}
// First update is done
_odom_first_update = false;
_last_predict_time = msg.header.stamp.toSec();
// Show updated particles
showDebugInformation();
}
