ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# How to do a frequency analysis of some velocity profiles?

Hello

I have some question regarding analysing some post-processing data. I have extracted a linear and angular velocity of the robot moving around. The velocity is coming from AMCL and angular velocity is calculating with the help of IMU unit (just use the position/orientation) and AMCL as well. I made different bag files( so different robot runs) and I have significant peeks (noises) in both profiles( in linear velocity not so much noises bit the angular velocity have significant noises). So thought a frequency analysis of the velocity profiles might reveal more high components in both velocity profiles. But dont know exactly how to do it? Also where this noises can come from??

Im posting one of my profiles. Angular speed

And the linear one!!

This IS the code for the speed calculation

void 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 = (curr_theta - prev_theta);
if (dist_theta < 0){
dist_theta = dist_theta * -1;
}
double dist_x = (curr_x - prev_x);
if (dist_x < 0){
dist_x = dist_x * -1;
}
double dist_y = (curr_y - prev_y);
if (dist_y < 0){
dist_y = dist_y * -1;
}
double dist_tot = sqrt((dist_x * dist_x) + (dist_y * dist_y));// * 10;
double speed = (dist_tot / time_dif);// * 3.6;
double speed_theta = dist_theta /time_dif;
if (speed < 0.12){
speed = 0;
zero_count++;
}
if (speed_theta < 0.07){
speed_theta = 0;
}

ROS_INFO("linear speed= %f, anglular speed=  %f", speed, speed_theta);
myfile <<  speed << "   " << speed_theta << "\n";
speed_count++;
speed_sum = speed_sum + speed;
speed_sum_theta = speed_sum_theta + speed_theta;
if (speed_count == 20){
speed = speed_sum / speed_count;
speed_theta = speed_sum_theta / speed_count;
if (zero_count > 7){
speed = 0;
}
speed_count = 0;
speed_sum = 0;
speed_sum_theta = 0;
zero_count = 0;

count = count + 1;
}

}
}

prev_theta = curr_theta;
prev_x = curr_x;
prev_y = curr_y;

time_prev = time_now;

}


Frames.pdf

edit retag close merge delete

Why are you trying to calculate velocities based on the positon/orientation from AMCL?

( 2012-10-08 17:14:30 -0600 )edit

I just use the velocity formula to calculate it, And for that I need a time and the position(which I get from amcl). Only that . Whats wrong with that??

( 2012-10-08 18:13:22 -0600 )edit

Sort by ยป oldest newest most voted

Amcl has the ability to "snap" the odometry to a new reference point. This can produce the uneven results you're seeing. (It's as if your robot instantly jumped to the new position, so your output is the true instantaneous velocity according to AMCL.)

Typically, Odometry is actually fairly well estimated velocities and angular velocities integrated. AMCL is there to handle the accumulated integral error. Most applications do not require more accurate velocities and angular velocities than wheel encoders/gyros/IMUs produce. Are you trying to calibrate the system in an online manner?

Also, I suspect that the large jumps in angular velocity may be subtracting radians instead of quaternions. The angular difference between 3.1 and -3.1 is 0.08 radians, not -6.2 radians (Unless your robot spins very quickly).

more

Sorry. So dont know what you mean exactly? SO if not use amcl than how to calculate the speed?I can post my code for speed calculation

( 2012-10-08 19:15:10 -0600 )edit

Do you have an /odom topic? That should include an estimate of linear and angular velocity. If not, using your current method on /odom instead of /amcl_pose should produce much better results.

( 2012-10-08 19:23:47 -0600 )edit

No, I dont have odom topic. Only amcl . So what you suggest??

( 2012-10-08 19:25:59 -0600 )edit

What publishes the tf estimate of odometry from your robot? AMCL takes in a laser scan, odometry (through tf), and a map, and publishes tf and amcl_pose. Does your robot have wheel odometry (encoders, IMU)?

( 2012-10-08 19:28:39 -0600 )edit

My robot has IMU, laser scan and kinect. How to see what publishes the tf estimate of odometry from the robot??

( 2012-10-08 19:38:10 -0600 )edit

Is this a Turtlebot?

On any robot, you can view the various rates and publishers of /tf by using the view_frames tool: http://www.ros.org/wiki/tf/Tutorials/Introduction%20to%20tf#Using_view_frames

( 2012-10-08 19:41:11 -0600 )edit

No. No Im using fuerte and my robot is just a electrical wheelchair.

( 2012-10-08 19:48:56 -0600 )edit

I dont have that . Have Broadcaster : amcl

( 2012-10-08 20:31:22 -0600 )edit