Robot_pose_ekf does not take into account the velocity commands
Hello
Im also ROS users and doing some research in assist robotics. Im using Fuerte and Ubuntu 10.04. Have somee problem with the robot_pose_ekf package. My platform has wheel encoders and IMU , laser and Kinect. So when using the robot_pose_ekf package it does not take into account the velocity commands sent to the robot when updating the filter. I have topics from encoders contains velocity massages but robot_pose_ekf it does not take into account.
So I though to modify the code of the robot_pose_ekf stack and add the velocity information. I'm not sure if that's possible. I've looked the code and seems that it only works with poses, but I'm not sure.
I already have an node for subscribe to odom topic. This my node
void odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
//first, we'll publish the transform over tf
odom_trans.transform.translation.x = odom_raw.pose.pose.position.x = msg->pose.pose.position.x;
odom_trans.transform.translation.y = odom_raw.pose.pose.position.y = msg->pose.pose.position.y;
odom_trans.transform.rotation = odom_raw.pose.pose.orientation = msg->pose.pose.orientation;
odom_raw.twist = msg->twist;
odom_trans.header.stamp = odom_raw.header.stamp = msg->header.stamp;
pub2.publish(odom_raw);
static tf::TransformBroadcaster br;
}
void poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
odom.pose.pose.position.x = msg->pose.pose.position.x;
odom.pose.pose.position.y = msg->pose.pose.position.y;
odom.pose.pose.orientation = msg->pose.pose.orientation;
odom.header.stamp = msg->header.stamp;
//Speed calculation
delta_t = ros::Time::now().toSec() - time_last;
odom.twist.twist.linear.x = (msg->pose.pose.position.x-xlast)/delta_t;
odom.twist.twist.linear.y = (msg->pose.pose.position.y-ylast)/delta_t;
curr_yaw = tf::getYaw(msg->pose.pose.orientation);
odom.twist.twist.angular.z = normalize(curr_yaw-yaw_last)/delta_t;
pub.publish(odom);
xlast = msg->pose.pose.position.x; ylast = msg->pose.pose.position.y; yaw_last = curr_yaw;
time_last = ros::Time::now().toSec();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ekf_encoders_odom");
ros::NodeHandle nh;
initialize();
ros::Subscriber sub = nh.subscribe("odom", 1, odomCallback);
pub2 = nh.advertise<nav_msgs::Odometry>("raw_odometry", 1);
ros::Rate rate(1);
while(ros::ok())
{
ros::spinOnce();
rate.sleep();
}
return 0;
}
So do I need something else???
